Naze32 clone with Frysky receiver
Você não pode selecionar mais de 25 tópicos Os tópicos devem começar com uma letra ou um número, podem incluir traços ('-') e podem ter até 35 caracteres.

CThunk.h 6.1KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202
  1. /* General C++ Object Thunking class
  2. *
  3. * - allows direct callbacks to non-static C++ class functions
  4. * - keeps track for the corresponding class instance
  5. * - supports an optional context parameter for the called function
  6. * - ideally suited for class object receiving interrupts (NVIC_SetVector)
  7. *
  8. * Copyright (c) 2014-2015 ARM Limited
  9. *
  10. * Licensed under the Apache License, Version 2.0 (the "License");
  11. * you may not use this file except in compliance with the License.
  12. * You may obtain a copy of the License at
  13. *
  14. * http://www.apache.org/licenses/LICENSE-2.0
  15. *
  16. * Unless required by applicable law or agreed to in writing, software
  17. * distributed under the License is distributed on an "AS IS" BASIS,
  18. * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  19. * See the License for the specific language governing permissions and
  20. * limitations under the License.
  21. */
  22. #ifndef __CTHUNK_H__
  23. #define __CTHUNK_H__
  24. #define CTHUNK_ADDRESS 1
  25. #if defined(__CORTEX_M3) || defined(__CORTEX_M4) || defined(__thumb2__)
  26. #define CTHUNK_VARIABLES volatile uint32_t code[1]
  27. /**
  28. * CTHUNK disassembly for Cortex-M3/M4 (thumb2):
  29. * * ldm.w pc,{r0,r1,r2,pc}
  30. *
  31. * This instruction loads the arguments for the static thunking function to r0-r2, and
  32. * branches to that function by loading its address into PC.
  33. *
  34. * This is safe for both regular calling and interrupt calling, since it only touches scratch registers
  35. * which should be saved by the caller, and are automatically saved as part of the IRQ context switch.
  36. */
  37. #define CTHUNK_ASSIGMENT m_thunk.code[0] = 0x8007E89F
  38. #elif defined(__CORTEX_M0PLUS) || defined(__CORTEX_M0)
  39. /*
  40. * CTHUNK disassembly for Cortex M0 (thumb):
  41. * * push {r0,r1,r2,r3,r4,lr} save touched registers and return address
  42. * * movs r4,#4 set up address to load arguments from (immediately following this code block) (1)
  43. * * add r4,pc set up address to load arguments from (immediately following this code block) (2)
  44. * * ldm r4!,{r0,r1,r2,r3} load arguments for static thunk function
  45. * * blx r3 call static thunk function
  46. * * pop {r0,r1,r2,r3,r4,pc} restore scratch registers and return from function
  47. */
  48. #define CTHUNK_VARIABLES volatile uint32_t code[3]
  49. #define CTHUNK_ASSIGMENT do { \
  50. m_thunk.code[0] = 0x2404B51F; \
  51. m_thunk.code[1] = 0xCC0F447C; \
  52. m_thunk.code[2] = 0xBD1F4798; \
  53. } while (0)
  54. #else
  55. #error "Target is not currently suported."
  56. #endif
  57. /* IRQ/Exception compatible thunk entry function */
  58. typedef void (*CThunkEntry)(void);
  59. template<class T>
  60. class CThunk
  61. {
  62. public:
  63. typedef void (T::*CCallbackSimple)(void);
  64. typedef void (T::*CCallback)(void* context);
  65. inline CThunk(T *instance)
  66. {
  67. init(instance, NULL, NULL);
  68. }
  69. inline CThunk(T *instance, CCallback callback)
  70. {
  71. init(instance, callback, NULL);
  72. }
  73. ~CThunk() {
  74. }
  75. inline CThunk(T *instance, CCallbackSimple callback)
  76. {
  77. init(instance, (CCallback)callback, NULL);
  78. }
  79. inline CThunk(T &instance, CCallback callback)
  80. {
  81. init(instance, callback, NULL);
  82. }
  83. inline CThunk(T &instance, CCallbackSimple callback)
  84. {
  85. init(instance, (CCallback)callback, NULL);
  86. }
  87. inline CThunk(T &instance, CCallback callback, void* context)
  88. {
  89. init(instance, callback, context);
  90. }
  91. inline void callback(CCallback callback)
  92. {
  93. m_callback = callback;
  94. }
  95. inline void callback(CCallbackSimple callback)
  96. {
  97. m_callback = (CCallback)callback;
  98. }
  99. inline void context(void* context)
  100. {
  101. m_thunk.context = (uint32_t)context;
  102. }
  103. inline void context(uint32_t context)
  104. {
  105. m_thunk.context = context;
  106. }
  107. inline uint32_t entry(void)
  108. {
  109. return (((uint32_t)&m_thunk)|CTHUNK_ADDRESS);
  110. }
  111. /* get thunk entry point for connecting rhunk to an IRQ table */
  112. inline operator CThunkEntry(void)
  113. {
  114. return (CThunkEntry)entry();
  115. }
  116. /* get thunk entry point for connecting rhunk to an IRQ table */
  117. inline operator uint32_t(void)
  118. {
  119. return entry();
  120. }
  121. /* simple test function */
  122. inline void call(void)
  123. {
  124. (((CThunkEntry)(entry()))());
  125. }
  126. private:
  127. T* m_instance;
  128. volatile CCallback m_callback;
  129. // TODO: this needs proper fix, to refactor toolchain header file and all its use
  130. // PACKED there is not defined properly for IAR
  131. #if defined (__ICCARM__)
  132. typedef __packed struct
  133. {
  134. CTHUNK_VARIABLES;
  135. volatile uint32_t instance;
  136. volatile uint32_t context;
  137. volatile uint32_t callback;
  138. volatile uint32_t trampoline;
  139. } CThunkTrampoline;
  140. #else
  141. typedef struct
  142. {
  143. CTHUNK_VARIABLES;
  144. volatile uint32_t instance;
  145. volatile uint32_t context;
  146. volatile uint32_t callback;
  147. volatile uint32_t trampoline;
  148. } __attribute__((__packed__)) CThunkTrampoline;
  149. #endif
  150. static void trampoline(T* instance, void* context, CCallback* callback)
  151. {
  152. if(instance && *callback) {
  153. (static_cast<T*>(instance)->**callback)(context);
  154. }
  155. }
  156. volatile CThunkTrampoline m_thunk;
  157. inline void init(T *instance, CCallback callback, void* context)
  158. {
  159. /* remember callback - need to add this level of redirection
  160. as pointer size for member functions differs between platforms */
  161. m_callback = callback;
  162. /* populate thunking trampoline */
  163. CTHUNK_ASSIGMENT;
  164. m_thunk.context = (uint32_t)context;
  165. m_thunk.instance = (uint32_t)instance;
  166. m_thunk.callback = (uint32_t)&m_callback;
  167. m_thunk.trampoline = (uint32_t)&trampoline;
  168. __ISB();
  169. __DSB();
  170. }
  171. };
  172. #endif/*__CTHUNK_H__*/