1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
|
/* Copyright (c) 2014 Nordic Semiconductor. All Rights Reserved.
*
* The information contained herein is property of Nordic Semiconductor ASA.
* Terms and conditions of usage are described in detail in NORDIC
* SEMICONDUCTOR STANDARD SOFTWARE LICENSE AGREEMENT.
*
* Licensees are granted free, non-transferable use of the information. NO
* WARRANTY of ANY KIND is provided. This heading must NOT be removed from
* the file.
*
*/
#include <sdk/libraries/bootloader_dfu/dfu_ble_svc.h>
#include <string.h>
#include <sdk/softdevice/s130/headers/nrf_error.h>
#include <sdk/libraries/crc16/crc16.h>
#if defined ( __CC_ARM )
static dfu_ble_peer_data_t m_peer_data __attribute__((section("NoInit"), zero_init)); /**< This variable should be placed in a non initialized RAM section in order to be valid upon soft reset from application into bootloader. */
static uint16_t m_peer_data_crc __attribute__((section("NoInit"), zero_init)); /**< CRC variable to ensure the integrity of the peer data provided. */
#elif defined ( __GNUC__ )
__attribute__((section(".noinit"))) static dfu_ble_peer_data_t m_peer_data; /**< This variable should be placed in a non initialized RAM section in order to be valid upon soft reset from application into bootloader. */
__attribute__((section(".noinit"))) static uint16_t m_peer_data_crc; /**< CRC variable to ensure the integrity of the peer data provided. */
#elif defined ( __ICCARM__ )
__no_init static dfu_ble_peer_data_t m_peer_data @ 0x20003F80; /**< This variable should be placed in a non initialized RAM section in order to be valid upon soft reset from application into bootloader. */
__no_init static uint16_t m_peer_data_crc @ 0x20003F80 + sizeof(dfu_ble_peer_data_t); /**< CRC variable to ensure the integrity of the peer data provided. */
#endif
/**@brief Function for setting the peer data from application in bootloader before reset.
*
* @param[in] p_peer_data Pointer to the peer data containing keys for the connection.
*
* @retval NRF_SUCCES The data was set succesfully.
* @retval NRF_ERROR_NULL If a null pointer was passed as argument.
*/
static uint32_t dfu_ble_peer_data_set(dfu_ble_peer_data_t * p_peer_data)
{
if (p_peer_data == NULL)
{
return NRF_ERROR_NULL;
}
uint32_t src = (uint32_t)p_peer_data;
uint32_t dst = (uint32_t)&m_peer_data;
// Calculating length in order to check if destination is residing inside source.
// Source inside the the destination (calculation underflow) is safe a source is read before
// written to destination so that when destination grows into source, the source data is no
// longer needed.
uint32_t len = dst - src;
if (src == dst)
{
// Do nothing as source and destination are identical, just calculate crc below.
}
else if (len < sizeof(dfu_ble_peer_data_t))
{
uint32_t i = 0;
dst += sizeof(dfu_ble_peer_data_t);
src += sizeof(dfu_ble_peer_data_t);
// Copy byte wise backwards when facing overlapping structures.
while (i++ <= sizeof(dfu_ble_peer_data_t))
{
*((uint8_t *)dst--) = *((uint8_t *)src--);
}
}
else
{
memcpy((void *)dst, (void *)src, sizeof(dfu_ble_peer_data_t));
}
m_peer_data_crc = crc16_compute((uint8_t *)&m_peer_data, sizeof(m_peer_data), NULL);
return NRF_SUCCESS;
}
/**@brief Function for handling second stage of SuperVisor Calls (SVC).
*
* @details The function will use svc_num to call the corresponding SVC function.
*
* @param[in] svc_num SVC number for function to be executed
* @param[in] p_svc_args Argument list for the SVC.
*
* @return This function returns the error value of the SVC return. For further details, please
* refer to the details of the SVC implementation itself.
* @ref NRF_ERROR_SVC_HANDLER_MISSING is returned if no SVC handler is implemented for the
* provided svc_num.
*/
void C_SVC_Handler(uint8_t svc_num, uint32_t * p_svc_args)
{
switch (svc_num)
{
case DFU_BLE_SVC_PEER_DATA_SET:
p_svc_args[0] = dfu_ble_peer_data_set((dfu_ble_peer_data_t *)p_svc_args[0]);
break;
default:
p_svc_args[0] = NRF_ERROR_SVC_HANDLER_MISSING;
break;
}
}
/**@brief Function for handling the first stage of SuperVisor Calls (SVC) in assembly.
*
* @details The function will use the link register (LR) to determine the stack (PSP or MSP) to be
* used and then decode the SVC number afterwards. After decoding the SVC number then
* @ref C_SVC_Handler is called for further processing of the SVC.
*/
#if defined ( __CC_ARM )
__asm void SVC_Handler(void)
{
EXC_RETURN_CMD_PSP EQU 0xFFFFFFFD ; EXC_RETURN using PSP for ARM Cortex. If Link register contains this value it indicates the PSP was used before the SVC, otherwise the MSP was used.
IMPORT C_SVC_Handler
LDR R0, =EXC_RETURN_CMD_PSP ; Load the EXC_RETURN into R0 to be able to compare against LR to determine stack pointer used.
CMP R0, LR ; Compare the link register with R0. If equal then PSP was used, otherwise MSP was used before SVC.
BNE UseMSP ; Branch to code fetching SVC arguments using MSP.
MRS R1, PSP ; Move PSP into R1.
B Call_C_SVC_Handler ; Branch to Call_C_SVC_Handler below.
UseMSP
MRS R1, MSP ; MSP was used, therefore Move MSP into R1.
Call_C_SVC_Handler
LDR R0, [R1, #24] ; The arguments for the SVC was stacked. R1 contains Stack Pointer, the values stacked before SVC are R0, R1, R2, R3, R12, LR, PC (Return address), xPSR.
; R1 contains current SP so the PC of the stacked frame is at SP + 6 words (24 bytes). We load the PC into R0.
SUBS R0, #2 ; The PC before the SVC is in R0. We subtract 2 to get the address prior to the instruction executed where the SVC number is located.
LDRB R0, [R0] ; SVC instruction low octet: Load the byte at the address before the PC to fetch the SVC number.
LDR R2, =C_SVC_Handler ; Load address of C implementation of SVC handler.
BX R2 ; Branch to C implementation of SVC handler. R0 is now the SVC number, R1 is the StackPointer where the arguments (R0-R3) of the original SVC are located.
ALIGN
}
#elif defined ( __GNUC__ )
void __attribute__ (( naked )) SVC_Handler(void)
{
const uint32_t exc_return = 0xFFFFFFFD; // EXC_RETURN using PSP for ARM Cortex. If Link register contains this value it indicates the PSP was used before the SVC, otherwise the MSP was used.
__asm volatile(
"cmp lr, %0\t\n" // Compare the link register with argument 0 (%0), which is exc_return. If equal then PSP was used, otherwise MSP was used before SVC.
"bne UseMSP\t\n" // Branch to code fetching SVC arguments using MSP.
"mrs r1, psp\t\n" // Move PSP into R1.
"b Call_C_SVC_Handler\t\n" // Branch to Call_C_SVC_Handler below.
"UseMSP: \t\n" //
"mrs r1, msp\t\n" // MSP was used, therefore Move MSP into R1.
"Call_C_SVC_Handler: \t\n" //
"ldr r0, [r1, #24]\t\n" // The arguments for the SVC was stacked. R1 contains Stack Pointer, the values stacked before SVC are R0, R1, R2, R3, R12, LR, PC (Return address), xPSR.
// R1 contains current SP so the PC of the stacked frame is at SP + 6 words (24 bytes). We load the PC into R0.
"sub r0, r0, #2\t\n" // The PC before the SVC is in R0. We subtract 2 to get the address prior to the instruction executed where the SVC number is located.
"ldrb r0, [r0]\t\n" // SVC instruction low octet: Load the byte at the address before the PC to fetch the SVC number.
"bx %1\t\n" // Branch to C implementation of SVC handler, argument 1 (%1). R0 is now the SVC number, R1 is the StackPointer where the arguments (R0-R3) of the original SVC are located.
".align\t\n"
:: "r" (exc_return), "r" (C_SVC_Handler) // Argument list for the gcc assembly. exc_return is %0, C_SVC_Handler is %1.
: "r0", "r1" // List of register maintained manually.
);
}
#elif defined ( __ICCARM__ )
void SVC_Handler(void)
{
asm("movs r0, #0x02\n" // Load 0x02 into R6 to prepare for exec return test.
"mvns r0, r0\n" // Invert R0 to obtain exec return code using PSP for ARM Cortex.
"cmp lr, r0\n" // Compare the link register with argument 0 (%0), which is exc_return. If equal then PSP was used, otherwise MSP was used before SVC.
"bne UseMSP\n" // Branch to code fetching SVC arguments using MSP.
"mrs r1, psp\n" // Move PSP into R1.
"b Call_C_SVC_Handler\t\n" // Branch to Call_C_SVC_Handler below.
"UseMSP: \n" //
"mrs r1, msp\n" // MSP was used, therefore Move MSP into R1.
"Call_C_SVC_Handler: \n" //
"ldr r0, [r1, #24]\n" // The arguments for the SVC was stacked. R1 contains Stack Pointer, the values stacked before SVC are R0, R1, R2, R3, R12, LR, PC (Return address), xPSR.
// R1 contains current SP so the PC of the stacked frame is at SP + 6 words (24 bytes). We load the PC into R0.
"subs r0, #0x02\n" // The PC before the SVC is in R0. We subtract 2 to get the address prior to the instruction executed where the SVC number is located.
"ldrb r0, [r0]\n" // SVC instruction low octet: Load the byte at the address before the PC to fetch the SVC number.
"bx %0\n" // Branch to C implementation of SVC handler, argument 1 (%1). R0 is now the SVC number, R1 is the StackPointer where the arguments (R0-R3) of the original SVC are located.
:: "r" (C_SVC_Handler) // Argument list for the gcc assembly. C_SVC_Handler is %0.
: "r0", "r1" // List of register maintained manually.
);
}
#else
#error Compiler not supported.
#endif
uint32_t dfu_ble_peer_data_get(dfu_ble_peer_data_t * p_peer_data)
{
uint16_t crc;
if (p_peer_data == NULL)
{
return NRF_ERROR_NULL;
}
crc = crc16_compute((uint8_t *)&m_peer_data, sizeof(m_peer_data), NULL);
if (crc != m_peer_data_crc)
{
return NRF_ERROR_INVALID_DATA;
}
*p_peer_data = m_peer_data;
// corrupt CRC to invalidate shared information.
m_peer_data_crc++;
return NRF_SUCCESS;
}
|
