123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154 |
- #include <stdbool.h>
- #include <stdint.h>
- #include "nrf_svc_function.h"
- #include "nrf_error.h"
- NRF_SECTION_DEF(svc_data, const nrf_svc_func_t);
- #define SVC_DATA_SECTION_ITEM_GET(i) NRF_SECTION_ITEM_GET(svc_data, nrf_svc_func_reg_t, (i))
- #define SVC_DATA_SECTION_ITEM_COUNT NRF_SECTION_ITEM_COUNT(svc_data, nrf_svc_func_reg_t)
- #ifdef __GNUC__
- void nrf_svc_handler_c(uint32_t* p_svc_args) __attribute__((used));
- #endif
- void nrf_svc_handler_c(uint32_t* p_svc_args)
- {
- uint32_t const num_funcs = SVC_DATA_SECTION_ITEM_COUNT;
- bool handled = false;
- uint8_t const svc_num = ((uint8_t *)p_svc_args[6])[-2];
- uint32_t svci_num = NRF_SVCI_SVC_NUM_INVALID;
-
- if (svc_num == NRF_SVCI_SVC_NUM)
- {
-
- svci_num = p_svc_args[4];
- }
- for (uint32_t i = 0; i < num_funcs; i++)
- {
- nrf_svc_func_reg_t const * func_reg = SVC_DATA_SECTION_ITEM_GET(i);
- if (func_reg->svc_num != svc_num)
- {
- continue;
- }
- if (svci_num != NRF_SVCI_SVC_NUM_INVALID && func_reg->svci_num != svci_num)
- {
- continue;
- }
-
- p_svc_args[0] = func_reg->func_ptr(p_svc_args[0], p_svc_args[1], p_svc_args[2], p_svc_args[3]);
- handled = true;
- break;
- }
- if (handled == false)
- {
-
- p_svc_args[0] = NRF_ERROR_SVC_HANDLER_MISSING;
- }
- }
- #if defined ( __CC_ARM )
- __ASM void SVC_Handler(void)
- {
- tst lr, #4 ; Test bit 2 of EXT_RETURN to see if MSP or PSP is used
- ite eq ;
- mrseq r0, MSP ; If equal, copy stack pointer from MSP
- mrsne r0, PSP ; If not equal, copy stack pointer from PSP
- B __cpp(nrf_svc_handler_c) ; Call C-implementation of handler. Exception stack frame in R0
- ALIGN 4 ; Protect with alignment
- }
- #elif defined ( __GNUC__ )
- void __attribute__((naked)) SVC_Handler(void)
- {
- __ASM volatile
- (
- "tst lr, #4\t\n"
- "ite eq\t\n"
- "mrseq r0, MSP\t\n"
- "mrsne r0, PSP\t\n"
- "b nrf_svc_handler_c\t\n"
- ".align\t\n"
- );
- }
- #elif defined ( __ICCARM__ )
- void SVC_Handler(void)
- {
- __ASM volatile
- (
- "tst lr, #4\t\n"
- "ite eq\t\n"
- "mrseq r0, MSP\t\n"
- "mrsne r0, PSP\t\n"
- "b nrf_svc_handler_c\t\n" :
- );
- }
- #else
- #error Compiler not supported.
- #endif
|