rtos.c
Go to the documentation of this file.
1 /*
2 MIT LICENSE
3 
4 Copyright 2014-2019 Inertial Sense, Inc. - http://inertialsense.com
5 
6 Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files(the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions :
7 
8 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
9 
10 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT, IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
11 */
12 
13 #include "FreeRTOS.h"
14 #include "task.h"
15 #include "rtos.h"
16 
17 #if !defined(PLATFORM_IS_EVB_2)
18 #include "globals.h"
19 #include "IS_internal.h"
20 #include "misc/maintenance.h"
21 #include "../../../hdw-src/uINS-3/IS_uINS/src/misc/debug_gpio.h"
22 #endif
23 #include "bootloaderApp.h"
24 
27 
28 
29 int createTask
30 (
31  int index,
32  pdTASK_CODE pxTaskCode,
33  const char * const pcName,
34  unsigned short usStackDepth,
35  void *pvParameters,
36  unsigned portBASE_TYPE uxPriority,
37  portTickType xTimeIncrement
38 )
39 {
40  if (!(index < RTOS_NUM_TASKS))
41  {
42  return -1;
43  }
44 
45  // Task call period - used within task and for CPU usage
46  g_rtos.task[index].periodMs = xTimeIncrement;
47 
48  // Task name
50  {
51  memset(&g_rtos.task[index].name[configMAX_TASK_NAME_LEN], 0, MAX_TASK_NAME_LEN - configMAX_TASK_NAME_LEN);
52  }
53  strncpy(g_rtos.task[index].name, pcName, configMAX_TASK_NAME_LEN);
54 
55  // Create RTOS Task
56  if (xTaskCreate(pxTaskCode, pcName, usStackDepth, pvParameters, uxPriority, (TaskHandle_t * const)&g_rtos.task[index].handle) != pdPASS)
57  {
58  return -1;
59  }
60  return 0;
61 }
62 
63 
64 void rtos_monitor(int numRtosTasks)
65 {
66  int i;
67 
68  #if (configGENERATE_RUN_TIME_STATS == 1)
69  uint32_t ulTotalRunTime = portGET_RUN_TIME_COUNTER_VALUE();
70  float fTotalRunTime = ((float)ulTotalRunTime) * 1e-2;
71  #endif // (configGENERATE_RUN_TIME_STATS == 1)
72 
73  TaskStatus_t status;
74 
75  for (i=0; i<numRtosTasks; i++)
76  {
77  // If updating free rtos, the idle and timer handles need to be passed through and set properly
78  // tasks.h: void vTaskStartScheduler( TaskHandle_t* idleTaskHandle, TaskHandle_t* timerTaskHandle ) PRIVILEGED_FUNCTION;
79  // timers.h: BaseType_t xTimerCreateTimerTask( TaskHandle_t* ) PRIVILEGED_FUNCTION;
80 
81  void* handle = (void*)g_rtos.task[i].handle;
82  if (handle)
83  {
84  vTaskGetInfo(handle, &status, 1, eRunning);
85  g_rtos.task[i].stackUnused = status.usStackHighWaterMark * sizeof(uint32_t);
87 
88  #if (configGENERATE_RUN_TIME_STATS == 1)
89  // Divide by zero
90  if (ulTotalRunTime)
91  g_rtos.task[i].cpuUsage = (float)status.ulRunTimeCounter / fTotalRunTime;
92  #endif // (configGENERATE_RUN_TIME_STATS == 1)
93  }
94  }
95 
97 }
98 
99 
100 void rtosResetStats(void)
101 {
102  for (size_t i = 0; i < RTOS_NUM_TASKS; i++)
103  {
104  g_rtos.task[i].maxRunTimeUs = 0;
105  }
106 }
107 
108 
110 {
111  // Sleep to reduce power consumption
113 }
114 
115 
117 {
118 #if !defined(PLATFORM_IS_EVB_2)
119  DBGPIO_TOGGLE(DBG_RTOS_TICK_HOOK_PIN); // Debug used to monitor RTOS tick execution
120 #endif
121 }
122 
123 
124 static void setGpbrWithTaskInfo(void)
125 {
126  uint32_t task;
127  if((uint32_t)xTaskGetCurrentTaskHandle() == (uint32_t)g_rtos.task[TASK_SAMPLE].handle){ task = TASK_SAMPLE; }
128  if((uint32_t)xTaskGetCurrentTaskHandle() == (uint32_t)g_rtos.task[TASK_NAV].handle){ task = TASK_NAV; }
129  if((uint32_t)xTaskGetCurrentTaskHandle() == (uint32_t)g_rtos.task[TASK_COMMUNICATIONS].handle){ task = TASK_COMMUNICATIONS; }
130  if((uint32_t)xTaskGetCurrentTaskHandle() == (uint32_t)g_rtos.task[TASK_MAINTENANCE].handle){ task = TASK_MAINTENANCE; }
131  GPBR->SYS_GPBR[GPBR_IDX_G1_TASK] = task;
134 }
135 
136 // implementation below acquired from
137 // http://www.freertos.org/Debugging-Hard-Faults-On-Cortex-M-Microcontrollers.html
138 void prvGetRegistersFromStack(uint32_t *pulFaultStackAddress);
139 void prvGetRegistersFromStack(uint32_t *pulFaultStackAddress)
140 {
142  GPBR->SYS_GPBR[GPBR_IDX_G5_LR] = pulFaultStackAddress[5]; // link reg
143  GPBR->SYS_GPBR[GPBR_IDX_PC] = pulFaultStackAddress[6]; // program counter
144  GPBR->SYS_GPBR[GPBR_IDX_PSR] = pulFaultStackAddress[7]; // program status register
146 }
147 
148 
149 void vApplicationStackOverflowHook(xTaskHandle* pxTask, signed char* pcTaskName)
150 {
151 #if defined(PLATFORM_IS_EVB_2) || defined(DEBUG)
152 
153 // printf("stack overflow %x %s\r\n", (unsigned int)pxTask, (portCHAR *)pcTaskName);
154  for (;;) { }
155 
156 #else // uINS
157 
161 
162 #endif
163 }
164 
165 
166 void vApplicationMallocFailedHook(uint32_t size, uint32_t remaining, uint32_t prevLR)
167 {
168 #if defined(PLATFORM_IS_EVB_2) || defined(DEBUG)
169 
170  /* Called if a call to pvPortMalloc() fails because there is insufficient
171  free memory available in the FreeRTOS heap. pvPortMalloc() is called
172  internally by FreeRTOS API functions that create tasks, queues, software
173  timers, and semaphores. The size of the FreeRTOS heap is set by the
174  configTOTAL_HEAP_SIZE configuration constant in FreeRTOSConfig.h. */
175 
176  /* Force an assert. */
177 // printf("stack overflow %x %s\r\n", (unsigned int)pxTask, (portCHAR *)pcTaskName);
178 // configASSERT( ( volatile void * ) NULL );
179  for (;;) { }
180 
181 #else // uINS
182 
184  GPBR->SYS_GPBR[GPBR_IDX_G4_FLASH_MIG] = size;
185  GPBR->SYS_GPBR[GPBR_IDX_G5_LR] = remaining;
186 
187  // Capture call stack
188  GPBR->SYS_GPBR[GPBR_IDX_PC] = prevLR; // program counter of call to malloc
189 
191 
192 #endif
193 }
194 
195 #if 1
196 void MemManage_Handler(void)
197 {
198 #if defined(PLATFORM_IS_EVB_2) || defined(DEBUG)
199 
200  for (;;) { }
201 
202 #else // uINS
203 
207 
208 #endif
209 }
210 
211 void BusFault_Handler(void)
212 {
213 #if defined(PLATFORM_IS_EVB_2) || defined(DEBUG)
214 
215  for (;;) { }
216 
217 #else // uINS
218 
222 
223 #endif
224 }
225 
227 {
228 #if defined(PLATFORM_IS_EVB_2) || defined(DEBUG)
229 
230  for (;;) { }
231 
232 #else // uINS
233 
237 
238 #endif
239 }
240 #endif
241 
242 
243 #if 1
244 
245 #pragma GCC push_options
246 #pragma GCC optimize ("O0")
247 
249 {
250 #if defined(PLATFORM_IS_EVB_2) || defined(DEBUG)
251 
252  for (;;) { }
253 
254 #else // uINS
255 
257 
258  __asm volatile
259  (
260  " tst lr, #4 \n"
261  " ite eq \n"
262  " mrseq r0, msp \n"
263  " mrsne r0, psp \n"
264  " ldr r1, [r0, #24] \n"
265  " ldr r2, handler_address_const_hard_fault \n"
266  " bx r2 \n"
267  " handler_address_const_hard_fault: .word prvGetRegistersFromStack \n"
268  );
269 
270 #endif
271 }
272 
273 
274 #else
275 
276 // implementation below acquired from
277 // https://mcuoneclipse.com/2012/11/24/debugging-hard-faults-on-arm-cortex-m/
287 void HardFault_HandlerC(unsigned long *hardfault_args){
288  volatile unsigned long stacked_r0;
289  volatile unsigned long stacked_r1;
290  volatile unsigned long stacked_r2;
291  volatile unsigned long stacked_r3;
292  volatile unsigned long stacked_r12;
293  volatile unsigned long stacked_lr;
294  volatile unsigned long stacked_pc;
295  volatile unsigned long stacked_psr;
296  volatile unsigned long _CFSR;
297  volatile unsigned long _HFSR;
298  volatile unsigned long _DFSR;
299  volatile unsigned long _AFSR;
300  volatile unsigned long _BFAR;
301  volatile unsigned long _MMAR;
302 
303  stacked_r0 = ((unsigned long)hardfault_args[0]);
304  stacked_r1 = ((unsigned long)hardfault_args[1]);
305  stacked_r2 = ((unsigned long)hardfault_args[2]);
306  stacked_r3 = ((unsigned long)hardfault_args[3]);
307  stacked_r12 = ((unsigned long)hardfault_args[4]);
308  stacked_lr = ((unsigned long)hardfault_args[5]);
309  stacked_pc = ((unsigned long)hardfault_args[6]);
310  stacked_psr = ((unsigned long)hardfault_args[7]);
311 
312  // Configurable Fault Status Register
313  // Consists of MMSR, BFSR and UFSR
314  _CFSR = (*((volatile unsigned long *)(0xE000ED28)));
315 
316  // Hard Fault Status Register
317  _HFSR = (*((volatile unsigned long *)(0xE000ED2C)));
318 
319  // Debug Fault Status Register
320  _DFSR = (*((volatile unsigned long *)(0xE000ED30)));
321 
322  // Auxiliary Fault Status Register
323  _AFSR = (*((volatile unsigned long *)(0xE000ED3C)));
324 
325  // Read the Fault Address Registers. These may not contain valid values.
326  // Check BFARVALID/MMARVALID to see if they are valid values
327  // MemManage Fault Address Register
328  _MMAR = (*((volatile unsigned long *)(0xE000ED34)));
329  // Bus Fault Address Register
330  _BFAR = (*((volatile unsigned long *)(0xE000ED38)));
331 
332  __asm("BKPT #0\n"); // Break into the debugger
333 }
334 
335 
336 void HardFault_Handler(void)
337 {
338  __asm volatile
339  (
340  " movs r0,#4 \n"
341  " movs r1, lr \n"
342  " tst r0, r1 \n"
343  " beq _MSP \n"
344  " mrs r0, psp \n"
345  " b _HALT \n"
346  "_MSP: \n"
347  " mrs r0, msp \n"
348  "_HALT: \n"
349  " ldr r1,[r0,#20] \n"
350  " b HardFault_HandlerC\n"
351  " bkpt #0 \n"
352  );
353 }
354 
355 #endif // 0
356 
357 
358 #pragma GCC pop_options
359 
360 static void HardFault_Test(void)
361 {
362 #if 0
363 #define TESTCASE 0
364 #if TESTCASE == 0
365  volatile uint8_t* ptr = (volatile uint8_t*)0x30000000; // access memory beyond the end of ram
366  volatile uint8_t val = *ptr;
367  // examine the psr
368 
369 #elif TESTCASE == 1
370  typedef void (*t_funcPtr)(void);
371  t_funcPtr MyFunc = (t_funcPtr)(0x0001000 | 0x0); // INVSTATE because jumping to valid address but thumb bit not set
372  MyFunc();
373  // examine the pc
374 
375 #elif TESTCASE == 2
376  volatile int i, j = 0;
377  SCB->CCR = 0x210; // enable div-by-zero trap in processor core
378  i =i/j; // i and j are 0 initialized -> Div/0
379 #endif
380 #endif
381 
382  for (;;) {}
383 }
384 
385 
386 
387 // enable to detect which handler was called as opposed to the default Dummy_Handler
388 #if defined(PLATFORM_IS_EVB_2)
389 
390 /* Cortex-M7 core handlers */
391 void NMI_Handler (void) { for (;;); }
392 //void HardFault_Handler (void) { for (;;); }
393 // void MemManage_Handler (void) { for (;;); }
394 // void BusFault_Handler (void) { for (;;); }
395 // void UsageFault_Handler (void) { for (;;); }
396 //void SVC_Handler (void) { for (;;); }
397 void DebugMon_Handler (void) { for (;;); }
398 //void PendSV_Handler (void) { for (;;); }
399 //void SysTick_Handler (void) { for (;;); }
400 
401 /* Peripherals handlers */
402 void SUPC_Handler (void) { for (;;); }
403 void RSTC_Handler (void) { for (;;); }
404 void RTC_Handler (void) { for (;;); }
405 //void RTT_Handler (void) { for (;;); }
406 void WDT_Handler (void) { for (;;); }
407 void PMC_Handler (void) { for (;;); }
408 void EFC_Handler (void) { for (;;); }
409 //void UART0_Handler (void) { for (;;); }
410 //void UART1_Handler (void) { for (;;); }
411 //void USART0_Handler (void) { for (;;); }
412 //void USART1_Handler (void) { for (;;); }
413 //void USART2_Handler (void) { for (;;); }
414 #ifdef _SAME70_HSMCI_INSTANCE_
415 void HSMCI_Handler (void) { for (;;); }
416 #endif /* _SAME70_HSMCI_INSTANCE_ */
417 void TWIHS0_Handler (void) { for (;;); }
418 void TWIHS1_Handler (void) { for (;;); }
419 //void SPI0_Handler (void) { for (;;); }
420 void SSC_Handler (void) { for (;;); }
421 //void TC0_Handler (void) { for (;;); }
422 //void TC1_Handler (void) { for (;;); }
423 void TC2_Handler (void) { for (;;); }
424 #ifdef _SAME70_TC1_INSTANCE_
425 void TC3_Handler (void) { for (;;); }
426 #endif /* _SAME70_TC1_INSTANCE_ */
427 #ifdef _SAME70_TC1_INSTANCE_
428 void TC4_Handler (void) { for (;;); }
429 #endif /* _SAME70_TC1_INSTANCE_ */
430 #ifdef _SAME70_TC1_INSTANCE_
431 // void TC5_Handler (void) { for (;;); }
432 #endif /* _SAME70_TC1_INSTANCE_ */
433 // void AFEC0_Handler (void) { for (;;); }
434 #ifdef _SAME70_DACC_INSTANCE_
435 void DACC_Handler (void) { for (;;); }
436 #endif /* _SAME70_DACC_INSTANCE_ */
437 void PWM0_Handler (void) { for (;;); }
438 void ICM_Handler (void) { for (;;); }
439 void ACC_Handler (void) { for (;;); }
440 // void USBHS_Handler (void) { for (;;); }
441 void MCAN0_Handler(void);
442 void MCAN1_Handler(void);
443 void MCAN0_Handler (void) { for (;;); }
444 void MCAN1_Handler (void) { for (;;); }
445 void GMAC_Handler (void) { for (;;); }
446 // void AFEC1_Handler (void) { for (;;); }
447 #ifdef _SAME70_TWIHS2_INSTANCE_
448 void TWIHS2_Handler (void) { for (;;); }
449 #endif /* _SAME70_TWIHS2_INSTANCE_ */
450 void SPI1_Handler (void) { for (;;); }
451 void QSPI_Handler (void) { for (;;); }
452 //void UART2_Handler (void) { for (;;); }
453 //void UART3_Handler (void) { for (;;); }
454 //void UART4_Handler (void) { for (;;); }
455 #ifdef _SAME70_TC2_INSTANCE_
456 void TC6_Handler (void) { for (;;); }
457 #endif /* _SAME70_TC2_INSTANCE_ */
458 #ifdef _SAME70_TC2_INSTANCE_
459 void TC7_Handler (void) { for (;;); }
460 #endif /* _SAME70_TC2_INSTANCE_ */
461 #ifdef _SAME70_TC2_INSTANCE_
462 void TC8_Handler (void) { for (;;); }
463 #endif /* _SAME70_TC2_INSTANCE_ */
464 // void TC9_Handler (void) { for (;;); }
465 void TC10_Handler (void) { for (;;); }
466 void TC11_Handler (void) { for (;;); }
467 void AES_Handler (void) { for (;;); }
468 void TRNG_Handler (void) { for (;;); }
469 // void XDMAC_Handler (void) { for (;;); }
470 void ISI_Handler (void) { for (;;); }
471 void PWM1_Handler (void) { for (;;); }
472 #ifdef _SAME70_SDRAMC_INSTANCE_
473 void SDRAMC_Handler (void) { for (;;); }
474 #endif /* _SAME70_SDRAMC_INSTANCE_ */
475 void RSWDT_Handler (void) { for (;;); }
476 
477 #endif // 0
#define GPBR_IDX_G4_FLASH_MIG
Definition: rtos.h:44
UBaseType_t uxCurrentPriority
Definition: task.h:135
#define GPBR_IDX_STATUS
Definition: rtos.h:40
void vApplicationMallocFailedHook(uint32_t size, uint32_t remaining, uint32_t prevLR)
Definition: rtos.c:166
uint32_t ulRunTimeCounter
Definition: task.h:137
uint16_t usStackHighWaterMark
Definition: task.h:139
void MemManage_Handler(void)
Definition: rtos.c:196
size_t xPortGetMinimumEverFreeHeapSize(void) PRIVILEGED_FUNCTION
Definition: heap_4.c:365
TaskHandle_t xTaskGetCurrentTaskHandle(void) PRIVILEGED_FUNCTION
void pmc_enable_sleepmode(uint8_t uc_type)
Enable Sleep Mode. Enter condition: (WFE or WFI) + (SLEEPDEEP bit = 0) + (LPM bit = 0) ...
Definition: pmc.c:1348
#define GPBR
(GPBR ) Base Address
Definition: same70j19.h:537
#define SYS_FAULT_STATUS_STACK_OVERFLOW
Definition: data_sets.h:3176
void RTC_Handler(void)
#define GPBR_IDX_G3_LINE_NUM
Definition: rtos.h:43
evb_rtos_info_t g_rtos
Definition: globals.c:27
Definition: task.h:73
void GMAC_Handler(void)
#define GPBR_IDX_PC
Definition: rtos.h:46
void HardFault_Handler(void)
Definition: rtos.c:248
void TWIHS0_Handler(void)
if(udd_ctrl_interrupt())
Definition: usbhs_device.c:688
#define portGET_RUN_TIME_COUNTER_VALUE()
rtos_task_t task[UINS_RTOS_NUM_TASKS]
Definition: data_sets.h:3335
static void setGpbrWithTaskInfo(void)
Definition: rtos.c:124
#define GPBR_IDX_G2_FILE_NUM
Definition: rtos.h:42
void TC11_Handler(void)
#define SYS_FAULT_STATUS_MALLOC_FAILED
Definition: data_sets.h:3175
void ACC_Handler(void)
void SUPC_Handler(void)
void SPI1_Handler(void)
void TC7_Handler(void)
#define DBGPIO_TOGGLE(pin)
Definition: ISConstants.h:193
uint32_t g_faultFileNumber
Definition: rtos.c:26
void vApplicationIdleHook(void)
Definition: rtos.c:109
uint32_t handle
Definition: data_sets.h:3319
void ISI_Handler(void)
void soft_reset_no_backup_register(void)
Definition: bootloaderApp.c:44
#define SCB
Definition: core_cm7.h:1599
static void HardFault_Test(void)
Definition: rtos.c:360
void DebugMon_Handler(void)
void TRNG_Handler(void)
#define GPBR_IDX_PSR
Definition: rtos.h:47
void DACC_Handler(void)
void PMC_Handler(void)
void EFC_Handler(void)
void rtosResetStats(void)
Definition: rtos.c:100
#define SYS_FAULT_STATUS_MEM_MANGE
Definition: data_sets.h:3173
float cpuUsage
Definition: data_sets.h:3316
void TC4_Handler(void)
void TWIHS1_Handler(void)
#define GPBR_IDX_G5_LR
Definition: rtos.h:45
void PWM0_Handler(void)
#define SYS_FAULT_STATUS_USAGE_FAULT
Definition: data_sets.h:3172
void TWIHS2_Handler(void)
#define pdPASS
Definition: projdefs.h:48
void * TaskHandle_t
Definition: task.h:62
#define SYS_FAULT_STATUS_HARD_FAULT
Definition: data_sets.h:3171
void rtos_monitor(int numRtosTasks)
Definition: rtos.c:64
void TC2_Handler(void)
void TC8_Handler(void)
void RSWDT_Handler(void)
#define portTickType
Definition: FreeRTOS.h:900
uint32_t freeHeapSize
Definition: data_sets.h:3326
void UsageFault_Handler(void)
Definition: rtos.c:226
void vApplicationStackOverflowHook(xTaskHandle *pxTask, signed char *pcTaskName)
Definition: rtos.c:149
void AES_Handler(void)
uint32_t maxRunTimeUs
Definition: data_sets.h:3307
#define MAX_TASK_NAME_LEN
Definition: data_sets.h:3286
void TC3_Handler(void)
uint32_t periodMs
Definition: data_sets.h:3301
#define xTaskHandle
Definition: FreeRTOS.h:901
void RSTC_Handler(void)
void PWM1_Handler(void)
void SSC_Handler(void)
void vApplicationTickHook(void)
Definition: rtos.c:116
#define portBASE_TYPE
Definition: portmacro.h:54
void MCAN1_Handler(void)
uint32_t g_faultLineNumber
Definition: rtos.c:25
#define GPBR_IDX_G1_TASK
Definition: rtos.h:41
void MCAN0_Handler(void)
#define pdTASK_CODE
Definition: FreeRTOS.h:922
void ICM_Handler(void)
void NMI_Handler(void)
void TC6_Handler(void)
void HSMCI_Handler(void)
void prvGetRegistersFromStack(uint32_t *pulFaultStackAddress)
Definition: rtos.c:139
#define configMAX_TASK_NAME_LEN
Definition: FreeRTOS.h:223
void vTaskGetInfo(TaskHandle_t xTask, TaskStatus_t *pxTaskStatus, BaseType_t xGetFreeStackSpace, eTaskState eState) PRIVILEGED_FUNCTION
void WDT_Handler(void)
uint32_t stackUnused
Definition: data_sets.h:3298
#define RTOS_NUM_TASKS
Definition: rtos.h:29
char name[MAX_TASK_NAME_LEN]
Definition: data_sets.h:3292
int createTask(int index, pdTASK_CODE pxTaskCode, const char *const pcName, unsigned short usStackDepth, void *pvParameters, unsigned portBASE_TYPE uxPriority, portTickType xTimeIncrement)
Definition: rtos.c:30
void QSPI_Handler(void)
#define SYS_FAULT_STATUS_BUS_FAULT
Definition: data_sets.h:3174
uint32_t priority
Definition: data_sets.h:3295
void SDRAMC_Handler(void)
void BusFault_Handler(void)
Definition: rtos.c:211
void TC10_Handler(void)


inertial_sense_ros
Author(s):
autogenerated on Sat Sep 19 2020 03:19:04