1 /*
2 * Copyright (C) 2016 The Android Open Source Project
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License");
5 * you may not use this file except in compliance with the License.
6 * You may obtain a copy of the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS,
12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13 * See the License for the specific language governing permissions and
14 * limitations under the License.
15 */
16
17 #include <plat/inc/cmsis.h>
18 #include <plat/inc/plat.h>
19 #include <syscall.h>
20 #include <string.h>
21 #include <seos.h>
22 #include <heap.h>
23 #include <cpu.h>
24
25
26 #define HARD_FAULT_DROPBOX_MAGIC_MASK 0xFFFFC000
27 #define HARD_FAULT_DROPBOX_MAGIC_VAL 0x31414000
28 #define HARD_FAULT_DROPBOX_MAGIC_HAVE_DROP 0x00002000
29 #define HARD_FAULT_DROPBOX_MAGIC_DATA_MASK 0x00001FFF
30
31 struct RamPersistedDataAndDropbox {
32 uint32_t magic; // and part of dropbox
33 uint32_t r[16];
34 uint32_t sr_hfsr_cfsr_lo;
35 uint32_t bits;
36 uint32_t RFU;
37 };
38
39 /* //if your device persists ram, you can use this instead:
40 * static struct RamPersistedDataAndDropbox* getPersistedData(void)
41 * {
42 * static struct RamPersistedDataAndDropbox __attribute__((section(".neverinit"))) dbx;
43 * return &dbx;
44 * }
45 */
46
getPersistedData(void)47 static struct RamPersistedDataAndDropbox* getPersistedData(void)
48 {
49 uint32_t bytes = 0;
50 void *loc = platGetPersistentRamStore(&bytes);
51
52 return bytes >= sizeof(struct RamPersistedDataAndDropbox) ? (struct RamPersistedDataAndDropbox*)loc : NULL;
53 }
54
getInitedPersistedData(void)55 static struct RamPersistedDataAndDropbox* getInitedPersistedData(void)
56 {
57 struct RamPersistedDataAndDropbox* dbx = getPersistedData();
58
59 if ((dbx->magic & HARD_FAULT_DROPBOX_MAGIC_MASK) != HARD_FAULT_DROPBOX_MAGIC_VAL) {
60 dbx->bits = 0;
61 dbx->magic = HARD_FAULT_DROPBOX_MAGIC_VAL;
62 }
63
64 return dbx;
65 }
66
cpuInit(void)67 void cpuInit(void)
68 {
69 /* set SVC to be highest possible priority */
70 NVIC_SetPriority(SVCall_IRQn, 0xff);
71
72 /* FPU on */
73 SCB->CPACR |= 0x00F00000;
74 }
75
76 //pack all our SR regs into 45 bits
cpuPackSrBits(uint32_t * dstLo,uint32_t * dstHi,uint32_t sr,uint32_t hfsr,uint32_t cfsr)77 static void cpuPackSrBits(uint32_t *dstLo, uint32_t *dstHi, uint32_t sr, uint32_t hfsr, uint32_t cfsr)
78 {
79 //mask of useful bits:
80 // SR: 11111111 00000000 11111101 11111111 (total of 23 bits)
81 // HFSR: 01000000 00000000 00000000 00000010 (total of 2 bits)
82 // CFSR: 00000011 00001111 10111111 10111111 (total of 20 bits)
83 // so our total is 45 bits. we pack this into 2 longs (for now)
84
85 sr &= 0xFF00FDFF;
86 hfsr &= 0x40000002;
87 cfsr &= 0x030FBFBF;
88
89 *dstLo = sr | ((cfsr << 4) & 0x00FF0000) | (hfsr >> 12) | (hfsr << 8);
90 *dstHi = ((cfsr & 0x01000000) >> 18) | ((cfsr & 0x02000000) >> 13) | (cfsr & 0x00000fff);
91 }
92
93 //unpack the SR bits
cpuUnpackSrBits(uint32_t srcLo,uint32_t srcHi,uint32_t * srP,uint32_t * hfsrP,uint32_t * cfsrP)94 static void cpuUnpackSrBits(uint32_t srcLo, uint32_t srcHi, uint32_t *srP, uint32_t *hfsrP, uint32_t *cfsrP)
95 {
96 *srP = srcLo & 0xFF00FDFF;
97 *hfsrP = ((srcLo << 12) & 0x40000000) | ((srcLo >> 8) & 0x00000002);
98 *cfsrP = ((srcLo & 0x00FB0000) >> 4) | (srcHi & 0x0FBF) | ((srcHi << 13) & 0x02000000) | ((srcHi << 18) & 0x01000000);
99 }
100
cpuInitLate(void)101 void cpuInitLate(void)
102 {
103 struct RamPersistedDataAndDropbox *dbx = getInitedPersistedData();
104
105 /* print and clear dropbox */
106 if (dbx->magic & HARD_FAULT_DROPBOX_MAGIC_HAVE_DROP) {
107 uint32_t i, hfsr, cfsr, sr;
108
109 cpuUnpackSrBits(dbx->sr_hfsr_cfsr_lo, dbx->magic & HARD_FAULT_DROPBOX_MAGIC_DATA_MASK, &sr, &hfsr, &cfsr);
110
111 osLog(LOG_INFO, "Hard Fault Dropbox not empty. Contents:\n");
112 for (i = 0; i < 16; i++)
113 osLog(LOG_INFO, " R%02lu = 0x%08lX\n", i, dbx->r[i]);
114 osLog(LOG_INFO, " SR = %08lX\n", sr);
115 osLog(LOG_INFO, " HFSR = %08lX\n", hfsr);
116 osLog(LOG_INFO, " CFSR = %08lX\n", cfsr);
117 }
118 dbx->magic &=~ HARD_FAULT_DROPBOX_MAGIC_HAVE_DROP;
119 }
120
cpuRamPersistentBitGet(uint32_t which)121 bool cpuRamPersistentBitGet(uint32_t which)
122 {
123 struct RamPersistedDataAndDropbox *dbx = getInitedPersistedData();
124
125 return (which < CPU_NUM_PERSISTENT_RAM_BITS) && ((dbx->bits >> which) & 1);
126 }
127
cpuRamPersistentBitSet(uint32_t which,bool on)128 void cpuRamPersistentBitSet(uint32_t which, bool on)
129 {
130 struct RamPersistedDataAndDropbox *dbx = getInitedPersistedData();
131
132 if (which < CPU_NUM_PERSISTENT_RAM_BITS) {
133 if (on)
134 dbx->bits |= (1ULL << which);
135 else
136 dbx->bits &=~ (1ULL << which);
137 }
138 }
139
cpuIntsOff(void)140 uint64_t cpuIntsOff(void)
141 {
142 uint32_t state;
143
144 asm volatile (
145 "mrs %0, PRIMASK \n"
146 "cpsid i \n"
147 :"=r"(state)
148 );
149
150 return state;
151 }
152
cpuIntsOn(void)153 uint64_t cpuIntsOn(void)
154 {
155 uint32_t state;
156
157 asm volatile (
158 "mrs %0, PRIMASK \n"
159 "cpsie i \n"
160 :"=r"(state)
161 );
162
163 return state;
164 }
165
cpuIntsRestore(uint64_t state)166 void cpuIntsRestore(uint64_t state)
167 {
168
169 asm volatile(
170 "msr PRIMASK, %0 \n"
171 ::"r"((uint32_t)state)
172 );
173 }
174
syscallHandler(uintptr_t * excRegs)175 static void __attribute__((used)) syscallHandler(uintptr_t *excRegs)
176 {
177 uint16_t *svcPC = ((uint16_t *)(excRegs[6])) - 1;
178 uint32_t svcNo = (*svcPC) & 0xFF;
179 uint32_t syscallNr = excRegs[0];
180 SyscallFunc handler;
181 va_list args_long = *(va_list*)(excRegs + 1);
182 uintptr_t *fastParams = excRegs + 1;
183 va_list args_fast = *(va_list*)(&fastParams);
184
185 if (svcNo > 1)
186 osLog(LOG_WARN, "Unknown SVC 0x%02lX called at 0x%08lX\n", svcNo, (unsigned long)svcPC);
187 else if (!(handler = syscallGetHandler(syscallNr)))
188 osLog(LOG_WARN, "Unknown syscall 0x%08lX called at 0x%08lX\n", (unsigned long)syscallNr, (unsigned long)svcPC);
189 else
190 handler(excRegs, svcNo ? args_fast : args_long);
191 }
192
193 void SVC_Handler(void);
SVC_Handler(void)194 void __attribute__((naked)) SVC_Handler(void)
195 {
196 asm volatile(
197 "tst lr, #4 \n"
198 "ite eq \n"
199 "mrseq r0, msp \n"
200 "mrsne r0, psp \n"
201 "b syscallHandler \n"
202 );
203 }
204
logHardFault(uintptr_t * excRegs,uintptr_t * otherRegs,bool tinyStack)205 static void __attribute__((used)) logHardFault(uintptr_t *excRegs, uintptr_t* otherRegs, bool tinyStack)
206 {
207 struct RamPersistedDataAndDropbox *dbx = getInitedPersistedData();
208 uint32_t i, hi;
209
210 for (i = 0; i < 4; i++)
211 dbx->r[i] = excRegs[i];
212 for (i = 0; i < 8; i++)
213 dbx->r[i + 4] = otherRegs[i];
214 dbx->r[12] = excRegs[4];
215 dbx->r[13] = (uint32_t)(excRegs + 8);
216 dbx->r[14] = excRegs[5];
217 dbx->r[15] = excRegs[6];
218
219 cpuPackSrBits(&dbx->sr_hfsr_cfsr_lo, &hi, excRegs[7], SCB->HFSR, SCB->CFSR);
220 dbx->magic |= HARD_FAULT_DROPBOX_MAGIC_HAVE_DROP | (hi & HARD_FAULT_DROPBOX_MAGIC_DATA_MASK);
221
222 if (!tinyStack) {
223 osLog(LOG_ERROR, "*HARD FAULT* SR = %08lX\n", (unsigned long)excRegs[7]);
224 osLog(LOG_ERROR, "R0 = %08lX R8 = %08lX\n", (unsigned long)excRegs[0], (unsigned long)otherRegs[4]);
225 osLog(LOG_ERROR, "R1 = %08lX R9 = %08lX\n", (unsigned long)excRegs[1], (unsigned long)otherRegs[5]);
226 osLog(LOG_ERROR, "R2 = %08lX R10 = %08lX\n", (unsigned long)excRegs[2], (unsigned long)otherRegs[6]);
227 osLog(LOG_ERROR, "R3 = %08lX R11 = %08lX\n", (unsigned long)excRegs[3], (unsigned long)otherRegs[7]);
228 osLog(LOG_ERROR, "R4 = %08lX R12 = %08lX\n", (unsigned long)otherRegs[0], (unsigned long)excRegs[4]);
229 osLog(LOG_ERROR, "R5 = %08lX SP = %08lX\n", (unsigned long)otherRegs[1], (unsigned long)(excRegs + 8));
230 osLog(LOG_ERROR, "R6 = %08lX LR = %08lX\n", (unsigned long)otherRegs[2], (unsigned long)excRegs[5]);
231 osLog(LOG_ERROR, "R7 = %08lX PC = %08lX\n", (unsigned long)otherRegs[3], (unsigned long)excRegs[6]);
232 osLog(LOG_ERROR, "HFSR= %08lX CFSR= %08lX\n", (unsigned long)SCB->HFSR, (unsigned long)SCB->CFSR);
233 }
234
235 //reset
236 SCB->AIRCR = 0x05FA0004;
237
238 //and in case somehow we do not, loop
239 while(1);
240 }
241
242 void HardFault_Handler(void);
243 static uint32_t __attribute__((used)) hfStack[16];
244
HardFault_Handler(void)245 void __attribute__((naked)) HardFault_Handler(void)
246 {
247 asm volatile(
248 "ldr r3, =__stack_bottom \n"
249 "cmp sp, r3 \n"
250 "itte le \n"
251 "ldrle sp, =hfStack + 64 \n"
252 "movle r2, #1 \n"
253 "movgt r2, #0 \n"
254 "tst lr, #4 \n"
255 "ite eq \n"
256 "mrseq r0, msp \n"
257 "mrsne r0, psp \n"
258 "push {r4-r11} \n"
259 "mov r1, sp \n"
260 "b logHardFault \n"
261 );
262 }
263
264
265