-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathport.c
1744 lines (1467 loc) · 58 KB
/
port.c
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
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include <stdarg.h>
#include "xil_printf.h"
/*
* FreeRTOS Kernel V10.4.3
* Copyright (C) 2020 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* 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:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* 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.
*
* https://www.FreeRTOS.org
* https://github.com/FreeRTOS
*
* 1 tab == 4 spaces!
*/
/* Standard includes. */
#include <stdlib.h>
/* Scheduler includes. */
#include "FreeRTOS.h"
#include "task.h"
/* Xilinx includes. */
#include "xscugic.h"
#ifndef configINTERRUPT_CONTROLLER_BASE_ADDRESS
#error configINTERRUPT_CONTROLLER_BASE_ADDRESS must be defined. See https://www.FreeRTOS.org/Using-FreeRTOS-on-Cortex-A-Embedded-Processors.html
#endif
#ifndef configINTERRUPT_CONTROLLER_CPU_INTERFACE_OFFSET
#error configINTERRUPT_CONTROLLER_CPU_INTERFACE_OFFSET must be defined. See https://www.FreeRTOS.org/Using-FreeRTOS-on-Cortex-A-Embedded-Processors.html
#endif
#ifndef configUNIQUE_INTERRUPT_PRIORITIES
#error configUNIQUE_INTERRUPT_PRIORITIES must be defined. See https://www.FreeRTOS.org/Using-FreeRTOS-on-Cortex-A-Embedded-Processors.html
#endif
#ifndef configSETUP_TICK_INTERRUPT
#error configSETUP_TICK_INTERRUPT() must be defined. See https://www.FreeRTOS.org/Using-FreeRTOS-on-Cortex-A-Embedded-Processors.html
#endif /* configSETUP_TICK_INTERRUPT */
#ifndef configMAX_API_CALL_INTERRUPT_PRIORITY
#error configMAX_API_CALL_INTERRUPT_PRIORITY must be defined. See https://www.FreeRTOS.org/Using-FreeRTOS-on-Cortex-A-Embedded-Processors.html
#endif
#if configMAX_API_CALL_INTERRUPT_PRIORITY == 0
#error configMAX_API_CALL_INTERRUPT_PRIORITY must not be set to 0
#endif
#if configMAX_API_CALL_INTERRUPT_PRIORITY > configUNIQUE_INTERRUPT_PRIORITIES
#error configMAX_API_CALL_INTERRUPT_PRIORITY must be less than or equal to configUNIQUE_INTERRUPT_PRIORITIES as the lower the numeric priority value the higher the logical interrupt priority
#endif
#if configUSE_PORT_OPTIMISED_TASK_SELECTION == 1
/* Check the configuration. */
#if( configMAX_PRIORITIES > 32 )
#error configUSE_PORT_OPTIMISED_TASK_SELECTION can only be set to 1 when configMAX_PRIORITIES is less than or equal to 32. It is very rare that a system requires more than 10 to 15 difference priorities as tasks that share a priority will time slice.
#endif
#endif /* configUSE_PORT_OPTIMISED_TASK_SELECTION */
/* In case security extensions are implemented. */
#if configMAX_API_CALL_INTERRUPT_PRIORITY <= ( configUNIQUE_INTERRUPT_PRIORITIES / 2 )
#error configMAX_API_CALL_INTERRUPT_PRIORITY must be greater than ( configUNIQUE_INTERRUPT_PRIORITIES / 2 )
#endif
/* Some vendor specific files default configCLEAR_TICK_INTERRUPT() in
portmacro.h. */
#ifndef configCLEAR_TICK_INTERRUPT
#define configCLEAR_TICK_INTERRUPT()
#endif
/* A critical section is exited when the critical section nesting count reaches
this value. */
#define portNO_CRITICAL_NESTING ( ( uint32_t ) 0 )
/* In all GICs 255 can be written to the priority mask register to unmask all
(but the lowest) interrupt priority. */
#define portUNMASK_VALUE ( 0xFFUL )
/* Tasks are not created with a floating point context, but can be given a
floating point context after they have been created. A variable is stored as
part of the tasks context that holds portNO_FLOATING_POINT_CONTEXT if the task
does not have an FPU context, or any other value if the task does have an FPU
context. */
#define portNO_FLOATING_POINT_CONTEXT ( ( StackType_t ) 0 )
/* Constants required to setup the initial task context. */
#define portINITIAL_SPSR ( ( StackType_t ) 0x1f ) /* System mode, ARM mode, IRQ enabled FIQ enabled. */
#define portTHUMB_MODE_BIT ( ( StackType_t ) 0x20 )
#define portINTERRUPT_ENABLE_BIT ( 0x80UL )
#define portTHUMB_MODE_ADDRESS ( 0x01UL )
/* Used by portASSERT_IF_INTERRUPT_PRIORITY_INVALID() when ensuring the binary
point is zero. */
#define portBINARY_POINT_BITS ( ( uint8_t ) 0x03 )
/* Masks all bits in the APSR other than the mode bits. */
#define portAPSR_MODE_BITS_MASK ( 0x1F )
/* The value of the mode bits in the APSR when the CPU is executing in user
mode. */
#define portAPSR_USER_MODE ( 0x10 )
/* The critical section macros only mask interrupts up to an application
determined priority level. Sometimes it is necessary to turn interrupt off in
the CPU itself before modifying certain hardware registers. */
#define portCPU_IRQ_DISABLE() \
__asm volatile ( "CPSID i" ::: "memory" ); \
__asm volatile ( "DSB" ); \
__asm volatile ( "ISB" );
#define portCPU_IRQ_ENABLE() \
__asm volatile ( "CPSIE i" ::: "memory" ); \
__asm volatile ( "DSB" ); \
__asm volatile ( "ISB" );
/* Macro to unmask all interrupt priorities. */
#define portCLEAR_INTERRUPT_MASK() \
{ \
portCPU_IRQ_DISABLE(); \
portICCPMR_PRIORITY_MASK_REGISTER = portUNMASK_VALUE; \
__asm volatile ( "DSB \n" \
"ISB \n" ); \
portCPU_IRQ_ENABLE(); \
}
#define portINTERRUPT_PRIORITY_REGISTER_OFFSET 0x400UL
#define portMAX_8_BIT_VALUE ( ( uint8_t ) 0xff )
#define portBIT_0_SET ( ( uint8_t ) 0x01 )
/* Let the user override the pre-loading of the initial LR with the address of
prvTaskExitError() in case it messes up unwinding of the stack in the
debugger. */
#define portTASK_RETURN_ADDRESS configTASK_RETURN_ADDRESS
/* The space on the stack required to hold the FPU registers. This is 32 64-bit
registers, plus a 32-bit status register. */
#define portFPU_REGISTER_WORDS ( ( 32 * 2 ) + 1 )
/*-----------------------------------------------------------*/
/*
* Initialise the interrupt controller instance.
*/
static int32_t prvInitialiseInterruptController( void );
/* Ensure the interrupt controller instance variable is initialised before it is
* used, and that the initialisation only happens once.
*/
static int32_t prvEnsureInterruptControllerIsInitialised( void );
/*
* Starts the first task executing. This function is necessarily written in
* assembly code so is implemented in portASM.s.
*/
extern void vPortRestoreTaskContext( void );
extern void vPortHandleNewTask( void );
/*
* Used to catch tasks that attempt to return from their implementing function.
*/
static void prvTaskExitError( void );
/*
* The instance of the interrupt controller used by this port. This is required
* by the Xilinx library API functions.
*/
extern XScuGic xInterruptController;
/*
* If the application provides an implementation of vApplicationIRQHandler(),
* then it will get called directly without saving the FPU registers on
* interrupt entry, and this weak implementation of
* vApplicationFPUSafeIRQHandler() is just provided to remove linkage errors -
* it should never actually get called so its implementation contains a
* call to configASSERT() that will always fail.
*
* If the application provides its own implementation of
* vApplicationFPUSafeIRQHandler() then the implementation of
* vApplicationIRQHandler() provided in portASM.S will save the FPU registers
* before calling it.
*
* Therefore, if the application writer wants FPU registers to be saved on
* interrupt entry their IRQ handler must be called
* vApplicationFPUSafeIRQHandler(), and if the application writer does not want
* FPU registers to be saved on interrupt entry their IRQ handler must be
* called vApplicationIRQHandler().
*/
void vApplicationFPUSafeIRQHandler( uint32_t ulICCIAR ) __attribute__((weak) );
/*-----------------------------------------------------------*/
/* A variable is used to keep track of the critical section nesting. This
variable has to be stored as part of the task context and must be initialised to
a non zero value to ensure interrupts don't inadvertently become unmasked before
the scheduler starts. As it is stored as part of the task context it will
automatically be set to 0 when the first task is started. */
volatile uint32_t ulCriticalNesting = 9999UL;
/* Saved as part of the task context. If ulPortTaskHasFPUContext is non-zero then
a floating point context must be saved and restored for the task. */
volatile uint32_t ulPortTaskHasFPUContext = pdFALSE;
/* Set to 1 to pend a context switch from an ISR. */
volatile uint32_t ulPortYieldRequired = pdFALSE;
/* Counts the interrupt nesting depth. A context switch is only performed if
if the nesting depth is 0. */
volatile uint32_t ulPortInterruptNesting = 0UL;
/*
* Global counter used for calculation of run time statistics of tasks.
* Defined only when the relevant option is turned on
*/
#if (configGENERATE_RUN_TIME_STATS==1)
volatile uint32_t ulHighFrequencyTimerTicks;
#endif
/* Used in the asm file. */
__attribute__(( used )) const uint32_t ulICCIAR = portICCIAR_INTERRUPT_ACKNOWLEDGE_REGISTER_ADDRESS;
__attribute__(( used )) const uint32_t ulICCEOIR = portICCEOIR_END_OF_INTERRUPT_REGISTER_ADDRESS;
__attribute__(( used )) const uint32_t ulICCPMR = portICCPMR_PRIORITY_MASK_REGISTER_ADDRESS;
__attribute__(( used )) const uint32_t ulMaxAPIPriorityMask = ( configMAX_API_CALL_INTERRUPT_PRIORITY << portPRIORITY_SHIFT );
/*-----------------------------------------------------------*/
/*
* See header file for description.
*/
StackType_t *pxPortInitialiseStack( StackType_t *pxTopOfStack, TaskFunction_t pxCode, void *pvParameters )
{
/* Setup the initial stack of the task. The stack is set exactly as
expected by the portRESTORE_CONTEXT() macro.
The fist real value on the stack is the status register, which is set for
system mode, with interrupts enabled. A few NULLs are added first to ensure
GDB does not try decoding a non-existent return address. */
*pxTopOfStack = ( StackType_t ) NULL;
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) NULL;
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) NULL;
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) portINITIAL_SPSR;
if( ( ( uint32_t ) pxCode & portTHUMB_MODE_ADDRESS ) != 0x00UL )
{
/* The task will start in THUMB mode. */
*pxTopOfStack |= portTHUMB_MODE_BIT;
}
pxTopOfStack--;
/* Next the return address, which in this case is the start of the task. */
*pxTopOfStack = ( StackType_t ) pxCode;
pxTopOfStack--;
/* Next all the registers other than the stack pointer. */
*pxTopOfStack = ( StackType_t ) portTASK_RETURN_ADDRESS; /* R14 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x12121212; /* R12 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x11111111; /* R11 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x10101010; /* R10 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x09090909; /* R9 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x08080808; /* R8 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x07070707; /* R7 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x06060606; /* R6 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x05050505; /* R5 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x04040404; /* R4 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x03030303; /* R3 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x02020202; /* R2 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) 0x01010101; /* R1 */
pxTopOfStack--;
*pxTopOfStack = ( StackType_t ) pvParameters; /* R0 */
pxTopOfStack--;
/* The task will start with a critical nesting count of 0 as interrupts are
enabled. */
*pxTopOfStack = portNO_CRITICAL_NESTING;
#if( configUSE_TASK_FPU_SUPPORT == 1 )
{
/* The task will start without a floating point context. A task that
uses the floating point hardware must call vPortTaskUsesFPU() before
executing any floating point instructions. */
pxTopOfStack--;
*pxTopOfStack = portNO_FLOATING_POINT_CONTEXT;
}
#elif( configUSE_TASK_FPU_SUPPORT == 2 )
{
/* The task will start with a floating point context. Leave enough
space for the registers - and ensure they are initialised to 0. */
pxTopOfStack -= portFPU_REGISTER_WORDS;
memset( pxTopOfStack, 0x00, portFPU_REGISTER_WORDS * sizeof( StackType_t ) );
pxTopOfStack--;
*pxTopOfStack = pdTRUE;
ulPortTaskHasFPUContext = pdTRUE;
}
#else
{
#error Invalid configUSE_TASK_FPU_SUPPORT setting - configUSE_TASK_FPU_SUPPORT must be set to 1, 2, or left undefined.
}
#endif
return pxTopOfStack;
}
/*-----------------------------------------------------------*/
static void prvTaskExitError( void )
{
/* A function that implements a task must not exit or attempt to return to
its caller as there is nothing to return to. If a task wants to exit it
should instead call vTaskDelete( NULL ).
Artificially force an assert() to be triggered if configASSERT() is
defined, then stop here so application writers can catch the error. */
xil_printf("Warning: return statement has been called from task %s, deleting it\n",pcTaskGetName(NULL));
if (uxTaskGetNumberOfTasks() == 2)
{
xil_printf("Warning: Kernel does not have any task to manage other than idle task\n");
}
vTaskDelete( NULL );
}
/*-----------------------------------------------------------*/
BaseType_t xPortInstallInterruptHandler( uint8_t ucInterruptID, XInterruptHandler pxHandler, void *pvCallBackRef )
{
int32_t lReturn;
/* An API function is provided to install an interrupt handler */
lReturn = prvEnsureInterruptControllerIsInitialised();
if( lReturn == pdPASS )
{
lReturn = XScuGic_Connect( &xInterruptController, ucInterruptID, pxHandler, pvCallBackRef );
}
if( lReturn == XST_SUCCESS )
{
lReturn = pdPASS;
}
configASSERT( lReturn == pdPASS );
return lReturn;
}
/*-----------------------------------------------------------*/
static int32_t prvEnsureInterruptControllerIsInitialised( void )
{
static int32_t lInterruptControllerInitialised = pdFALSE;
int32_t lReturn;
/* Ensure the interrupt controller instance variable is initialised before
it is used, and that the initialisation only happens once. */
if( lInterruptControllerInitialised != pdTRUE )
{
lReturn = prvInitialiseInterruptController();
if( lReturn == pdPASS )
{
lInterruptControllerInitialised = pdTRUE;
}
}
else
{
lReturn = pdPASS;
}
return lReturn;
}
/*-----------------------------------------------------------*/
static int32_t prvInitialiseInterruptController( void )
{
BaseType_t xStatus;
XScuGic_Config *pxGICConfig;
/* Initialize the interrupt controller driver. */
pxGICConfig = XScuGic_LookupConfig( XPAR_SCUGIC_SINGLE_DEVICE_ID );
xStatus = XScuGic_CfgInitialize( &xInterruptController, pxGICConfig, pxGICConfig->CpuBaseAddress );
if( xStatus == XST_SUCCESS )
{
xStatus = pdPASS;
}
else
{
xStatus = pdFAIL;
}
configASSERT( xStatus == pdPASS );
return xStatus;
}
/*-----------------------------------------------------------*/
void vPortEnableInterrupt( uint8_t ucInterruptID )
{
int32_t lReturn;
/* An API function is provided to enable an interrupt in the interrupt
controller. */
lReturn = prvEnsureInterruptControllerIsInitialised();
if( lReturn == pdPASS )
{
XScuGic_Enable( &xInterruptController, ucInterruptID );
}
configASSERT( lReturn );
}
/*-----------------------------------------------------------*/
void vPortDisableInterrupt( uint8_t ucInterruptID )
{
int32_t lReturn;
/* An API function is provided to disable an interrupt in the interrupt
controller. */
lReturn = prvEnsureInterruptControllerIsInitialised();
if( lReturn == pdPASS )
{
XScuGic_Disable( &xInterruptController, ucInterruptID );
}
configASSERT( lReturn );
}
/*-----------------------------------------------------------*/
//___trainedData save and load mechanism___
#include "xsdps.h"
#define TRAINEDDATA_REALSIZE (sizeof(FAULTDETECTOR_region_t)*FAULTDETECTOR_MAX_CHECKS*FAULTDETECTOR_MAX_REGIONS+sizeof(u8)*FAULTDETECTOR_MAX_CHECKS)
#define SD_BLOCKSIZE 512
#define TRAINEDDATA_BLOCKS_SIZE ((TRAINEDDATA_REALSIZE / SD_BLOCKSIZE) + ((TRAINEDDATA_REALSIZE % SD_BLOCKSIZE) != 0))
#define SD_SECTOR_OFFSET 1024//204800
static XSdPs SdInstance;
u32 Sd_Sector = SD_SECTOR_OFFSET;
typedef struct __attribute__((__packed__)) {
FAULTDETECTOR_region_t trainedRegions[FAULTDETECTOR_MAX_CHECKS][FAULTDETECTOR_MAX_REGIONS];
u8 n_regions[FAULTDETECTOR_MAX_CHECKS];
volatile char padding [ TRAINEDDATA_BLOCKS_SIZE*512 - TRAINEDDATA_REALSIZE ];
} trainedData ;
int prvInitSd(XSdPs* SD_InstancePtr)
{
int Status;
XSdPs_Config *SdConfig;
/*
* Since block size is 512 bytes. File Size is 512*BlockCount.
*/
// u32 FileSize = (512*TRAINEDDATA_BLOCKS_SIZE); /* File Size is only up to 2MB */
/*
* Initialize the host controller
*/
SdConfig = XSdPs_LookupConfig(XPAR_XSDPS_0_DEVICE_ID);
if (NULL == SdConfig) {
return XST_FAILURE;
}
Status = XSdPs_CfgInitialize(SD_InstancePtr, SdConfig,
SdConfig->BaseAddress);
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
Status = XSdPs_CardInitialize(SD_InstancePtr);
if (Status != XST_SUCCESS) {
// xil_printf("\nSD CARD INIT FAILED. CHECK AN SD CARD IS IN THE SLOT. TRAINED DATA DUMP DISABLED UNTIL RESET\n");
return XST_FAILURE;
}
if (!(SdInstance.HCS)) Sd_Sector *= XSDPS_BLK_SIZE_512_MASK;
return XST_SUCCESS;
}
#ifdef __ICCARM__
#pragma data_alignment = 32
trainedData dumpedDataSdBuf;
#else
trainedData dumpedDataSdBuf __attribute__ ((aligned(32)));
#endif
int prvRestoreTrainedData(
#ifndef configFAULTDETECTOR_SOFTWARE
XFaultdetector* FaultDet_InstancePtr,
#endif
XSdPs* SD_InstancePtr) {
/*
* Read data from SD/eMMC.
*/
int Status;
Status = XSdPs_ReadPolled(SD_InstancePtr, Sd_Sector, TRAINEDDATA_BLOCKS_SIZE,
(u8*) (&dumpedDataSdBuf));
if (Status!=XST_SUCCESS) {
return XST_FAILURE;
}
#ifdef configFAULTDETECTOR_SOFTWARE
FAULTDETECTOR_SW_initRegions(dumpedDataSdBuf.trainedRegions, dumpedDataSdBuf.n_regions);
#else
FAULTDETECTOR_initRegions(FaultDet_InstancePtr, dumpedDataSdBuf.trainedRegions, dumpedDataSdBuf.n_regions);
#endif
return XST_SUCCESS;
}
void FAULTDET_StopRunMode();
int prvDumpTrainedData(
#ifndef configFAULTDETECTOR_SOFTWARE
XFaultdetector* FaultDet_InstancePtr,
#endif
XSdPs* SD_InstancePtr) {
#ifdef configFAULTDETECTOR_SOFTWARE
xil_printf("\nSTARTING TO DUMP DATA\n");
FAULTDETECTOR_SW_dumpRegions(dumpedDataSdBuf.trainedRegions, dumpedDataSdBuf.n_regions);
#else
FAULTDET_StopRunMode();
xil_printf("\nFAULT DETECTOR EXITED RUN MODE. STARTING TO DUMP DATA\n");
FAULTDETECTOR_dumpRegions(FaultDet_InstancePtr, dumpedDataSdBuf.trainedRegions, dumpedDataSdBuf.n_regions);
#endif
/*
* Write data to SD/eMMC.
*/
xil_printf("\nDUMPED DATA FROM FAULT DETECTOR SUCCESFULLY. WRITING TO SD\n");
int Status;
Status = XSdPs_WritePolled(SD_InstancePtr, Sd_Sector, TRAINEDDATA_BLOCKS_SIZE,
(u8*) (&dumpedDataSdBuf));
if (Status != XST_SUCCESS) {
return XST_FAILURE;
}
return XST_SUCCESS;
}
//_______________________________________________
//INTERRUPT SYSTEM FOR COPYING MODEL FROM FAULT DETECTOR
#include "xgpio.h"
//#include "xintc.h"
#ifdef XPAR_INTC_0_DEVICE_ID
#include "xintc.h"
#include <stdio.h>
#else
#include "xscugic.h"
#endif
#ifdef XPAR_INTC_0_DEVICE_ID
#define INTC_GPIO_INTERRUPT_ID XPAR_INTC_0_GPIO_0_VEC_ID
#define INTC_DEVICE_ID XPAR_INTC_0_DEVICE_ID
#define INTC XIntc
#define INTC_HANDLER XIntc_InterruptHandler
#else
#define INTC_GPIO_INTERRUPT_ID XPAR_FABRIC_AXI_GPIO_0_IP2INTC_IRPT_INTR
#define INTC_DEVICE_ID XPAR_SCUGIC_SINGLE_DEVICE_ID
#define INTC XScuGic
#define INTC_HANDLER XScuGic_InterruptHandler
#endif /* XPAR_INTC_0_DEVICE_ID */
#define GPIO_DEVICE_ID XPAR_GPIO_0_DEVICE_ID
#define GPIO_CHANNEL1 1
XGpio Gpio0; /* The Instance of the GPIO Driver */
INTC Intc; /* The Instance of the Interrupt Controller Driver */
static u16 GPIOGlobalIntrMask; /* GPIO channel mask that is needed by
* the Interrupt Handler */
static volatile u32 GPIOIntrFlag; /* Interrupt Handler Flag */
void BtnPressHandler(void *CallbackRef);
int GpioSetupIntrSystem(INTC *IntcInstancePtr, XGpio *InstancePtr,
u16 IntrId, u16 IntrMask)
{
int Result;
GPIOGlobalIntrMask = IntrMask;
#ifdef XPAR_INTC_0_DEVICE_ID
#ifndef TESTAPP_GEN
/*
* Initialize the interrupt controller driver so that it's ready to use.
* specify the device ID that was generated in xparameters.h
*/
Result = XIntc_Initialize(IntcInstancePtr, INTC_DEVICE_ID);
if (Result != XST_SUCCESS) {
return Result;
}
#endif /* TESTAPP_GEN */
/* Hook up interrupt service routine */
XIntc_Connect(IntcInstancePtr, IntrId,
(Xil_ExceptionHandler)BtnPressHandler, InstancePtr);
/* Enable the interrupt vector at the interrupt controller */
XIntc_Enable(IntcInstancePtr, IntrId);
#ifndef TESTAPP_GEN
/*
* Start the interrupt controller such that interrupts are recognized
* and handled by the processor
*/
Result = XIntc_Start(IntcInstancePtr, XIN_REAL_MODE);
if (Result != XST_SUCCESS) {
return Result;
}
#endif /* TESTAPP_GEN */
#else /* !XPAR_INTC_0_DEVICE_ID */
#ifndef TESTAPP_GEN
XScuGic_Config *IntcConfig;
/*
* Initialize the interrupt controller driver so that it is ready to
* use.
*/
IntcConfig = XScuGic_LookupConfig(INTC_DEVICE_ID);
if (NULL == IntcConfig) {
return XST_FAILURE;
}
Result = XScuGic_CfgInitialize(IntcInstancePtr, IntcConfig,
IntcConfig->CpuBaseAddress);
if (Result != XST_SUCCESS) {
return XST_FAILURE;
}
#endif /* TESTAPP_GEN */
XScuGic_SetPriorityTriggerType(IntcInstancePtr, IntrId,
0xA0, 0x3);
/*
* Connect the interrupt handler that will be called when an
* interrupt occurs for the device.
*/
Result = XScuGic_Connect(IntcInstancePtr, IntrId,
(Xil_ExceptionHandler)BtnPressHandler, InstancePtr);
if (Result != XST_SUCCESS) {
return Result;
}
/* Enable the interrupt for the GPIO device.*/
XScuGic_Enable(IntcInstancePtr, IntrId);
#endif /* XPAR_INTC_0_DEVICE_ID */
/*
* Enable the GPIO channel interrupts so that push button can be
* detected and enable interrupts for the GPIO device
*/
XGpio_InterruptEnable(InstancePtr, IntrMask);
XGpio_InterruptGlobalEnable(InstancePtr);
/*
* Initialize the exception table and register the interrupt
* controller handler with the exception table
*/
Xil_ExceptionInit();
Xil_ExceptionRegisterHandler(XIL_EXCEPTION_ID_INT,
(Xil_ExceptionHandler)INTC_HANDLER, IntcInstancePtr);
/* Enable non-critical exceptions */
//Xil_ExceptionEnable();
return XST_SUCCESS;
}
//________________________________
//fedit add
/* Initializes the scheduler. */
#include "xparameters.h"
#include "scheduler.h"
#define SCHEDULER_BASEADDR XPAR_SCHEDULER_0_S_AXI_BASEADDR
//#define INTC_DEVICE_ID XPAR_SCUGIC_SINGLE_DEVICE_ID
#define SCHEDULER_INTR XPAR_FABRIC_SCHEDULER_0_IRQ_INTR
//represents the data written to RAM by the scheduler on FPGA
typedef struct __attribute__((__packed__)) {
TCB_t * pxNextTcb;
u8 executionMode; //normal, reexecution due to fault, reexecution due to timing fail
u8 requiresFaultDetection;
u8 executionId;
// u8 padding;
} newTaskDescrStr;
#define NEWTASKDESCRPTR 0x10000000
//pointer to pxCurrentTCB_ptr in task.h
extern PRIVILEGED_DATA TCB_t * volatile pxCurrentTCB;
#define CPU_BASEADDR XPAR_SCUGIC_CPU_BASEADDR
#define FAULTDETECTOR_BASEADDR XPAR_RUN_0_S_AXI_CONTROL_BASEADDR
//commands used by the fault detector
#define COMMAND_INIT 1
#define COMMAND_TEST 2
#define COMMAND_TRAIN 3
#ifndef configFAULTDETECTOR_SOFTWARE
XFaultdetector FAULTDETECTOR_InstancePtr;
XFaultdetector FAULTDET_getInstancePtr() {
return FAULTDETECTOR_InstancePtr;
}
#endif
//contains the test/train points (AOV and metadata). This array is accessed by the fault detector on FPGA by its internal DMA.
FAULTDETECTOR_controlStr controlForFaultDet [configMAX_RT_TASKS] __attribute__((aligned(4096)));
#define FAULTDETECTOR_DEVICEID XPAR_FAULTDETECTOR_0_DEVICE_ID
void FAULTDET_dumpRegions() {
if (prvDumpTrainedData(
#ifndef configFAULTDETECTOR_SOFTWARE
&FAULTDETECTOR_InstancePtr,
#endif
&SdInstance)==XST_SUCCESS) {
xil_printf("SUCCESS\n");
} else {
xil_printf("FAILED\n");
}
}
//callback function called when the button connected to the XGpio CHANNEL1 in FPGA is pressed.
//it copies the model from the fault detector on FPGA.
volatile void BtnPressHandler(void *CallbackRef)
{
xPortSchedulerDisableIntr(); //disable scheduler interrupts in order to avoid interruptions from higher priority interrupts and also consequently new AOV (also train ones) being submitted to fault detector
XGpio *GpioPtr = (XGpio *)CallbackRef;
if (XGpio_DiscreteRead(&Gpio0, GPIO_CHANNEL1)!=0) {
xil_printf("\nBEGIN TRAINED DATA DUMP\n");
if (prvDumpTrainedData(
#ifndef configFAULTDETECTOR_SOFTWARE
&FAULTDETECTOR_InstancePtr,
#endif
&SdInstance)==XST_SUCCESS) {
xil_printf("SUCCESS\n");
} else {
xil_printf("FAILED\n");
}
}
/* Clear the Interrupt */
XGpio_InterruptClear(GpioPtr, GPIOGlobalIntrMask);
}
//fault detector initialisation and startup
void FAULTDET_init(u8 restoreTrainDataFromSd, FAULTDETECTOR_region_t trainedRegions[FAULTDETECTOR_MAX_CHECKS][FAULTDETECTOR_MAX_REGIONS], u8 n_regions[FAULTDETECTOR_MAX_CHECKS]) {
//setup GPIO interrupt to enable dump trained data to SD when the user presses a button
int sdStatus=prvInitSd(&SdInstance);
if (sdStatus==XST_SUCCESS) {
XGpio_Initialize(&Gpio0, GPIO_DEVICE_ID);
GpioSetupIntrSystem(&Intc, &Gpio0,
INTC_GPIO_INTERRUPT_ID,
GPIO_CHANNEL1);
}
#ifndef configFAULTDETECTOR_SOFTWARE
//setup FAULT DETECTOR
XFaultdetector_Config* configPtr=XFaultdetector_LookupConfig(FAULTDETECTOR_DEVICEID);
XFaultdetector_CfgInitialize(&FAULTDETECTOR_InstancePtr, configPtr);
XFaultdetector_Set_inputData(&FAULTDETECTOR_InstancePtr, (u32) (&controlForFaultDet));
#endif
//restore the model loaded from SD card
if (sdStatus==XST_SUCCESS && restoreTrainDataFromSd) {
prvRestoreTrainedData(
#ifndef configFAULTDETECTOR_SOFTWARE
&FAULTDETECTOR_InstancePtr,
#endif
&SdInstance);
} else {
//restore the model passed as function argument
#ifdef configFAULTDETECTOR_SOFTWARE
FAULTDETECTOR_SW_initRegions(trainedRegions, n_regions);
#else
FAULTDETECTOR_initRegions(&FAULTDETECTOR_InstancePtr, trainedRegions, n_regions);
#endif
}
}
#ifndef configFAULTDETECTOR_SOFTWARE
//start the fault detector in RUN mode, ready to read test/train point from RAM. Initialisation must be performed first.
void FAULTDET_start () {
FAULTDETECTOR_setModeRun(&FAULTDETECTOR_InstancePtr);
XFaultdetector_Start(&FAULTDETECTOR_InstancePtr);
}
#endif
//replace the previous fault detection model with the one passed as argument, to be used while the fault detector is running
void FAULTDET_hotUpdateRegions(FAULTDETECTOR_region_t trainedRegions[FAULTDETECTOR_MAX_CHECKS][FAULTDETECTOR_MAX_REGIONS], u8 n_regions[FAULTDETECTOR_MAX_CHECKS]) {
#ifdef configFAULTDETECTOR_SOFTWARE
FAULTDETECTOR_SW_initRegions(trainedRegions, n_regions);
#else
FAULTDET_StopRunMode();
FAULTDETECTOR_initRegions(&FAULTDETECTOR_InstancePtr, trainedRegions, n_regions);
FAULTDET_start ();
#endif
}
FAULTDETECTOR_controlStr* FAULTDET_initFaultDetection() {
//FAULTDET_resetFault(); //not needed, automatically done by the faultdetector on FPGA when a command from the same check but with different UniId is received
u8 taskId=pxCurrentTCB->uxTaskNumber-1;
#ifndef configFAULTDETECTOR_SOFTWARE
#ifndef configDISABLE_ONLINE_TRAINING
if (pxCurrentTCB->executionMode==EXECMODE_CURRJOB_FAULT) {
FAULTDETECTOR_getLastTestedPoint(&FAULTDETECTOR_InstancePtr, taskId, &(pxCurrentTCB->lastFault));
}
#endif
#endif
return &(controlForFaultDet[taskId]);
}
//wrapper around FAULTDET_dumpRegions, to obtain the last regions stored in the fault detector
void FAULTDET_dumpRegionsToDest(FAULTDETECTOR_region_t trainedRegions[FAULTDETECTOR_MAX_CHECKS][FAULTDETECTOR_MAX_REGIONS], u8 n_regions[FAULTDETECTOR_MAX_CHECKS]) {
#ifdef configFAULTDETECTOR_SOFTWARE
FAULTDETECTOR_SW_dumpRegions(trainedRegions, n_regions);
#else
FAULTDET_StopRunMode();
FAULTDETECTOR_dumpRegions(&FAULTDETECTOR_InstancePtr, trainedRegions, n_regions);
#endif
}
#ifndef configFAULTDETECTOR_SOFTWARE
//to be called when the fault detector is in RUN_MODE, waiting for new AOV: after processing all the pending AOVs, if they exists, it stops
void FAULTDET_StopRunMode() {
if (XFaultdetector_IsIdle(&FAULTDETECTOR_InstancePtr))
return;
FAULTDETECTOR_controlStr contr;
contr.command=1;
controlForFaultDet[0]=contr;
FAULTDETECTOR_startCopy(&FAULTDETECTOR_InstancePtr, 0);
while (!XFaultdetector_IsIdle(&FAULTDETECTOR_InstancePtr)) {};
}
//wrapper around FAULTDETECTOR_getLastTestedPoint, to get the last testpoint that has been processed information
void FAULTDET_getLastTestedPoint(FAULTDETECTOR_testpointDescriptorStr* dest) {
FAULTDETECTOR_getLastTestedPoint(&FAULTDETECTOR_InstancePtr, (pxCurrentTCB->uxTaskNumber)-1, dest);
}
//to manually reset a fault in a task in the fault detector, it is never used because the fault detector automatically resets the fault when a AOV with a different executionId is submitted
void FAULTDET_resetFault() {
FAULTDETECTOR_resetFault(&FAULTDETECTOR_InstancePtr, (pxCurrentTCB->uxTaskNumber)-1);
}
#endif
#ifndef configFAULTDETECTOR_SOFTWARE
//contains the last test details, FAULTDET_blockIfFaultDetectedInTask will wait until the last test stored in the fault detector matches this one
FAULTDETECTOR_testpointShortDescriptorStr lastRequestedTest[configMAX_RT_TASKS];
//waits until the last test stored in the fault detector matches the last test requested
void FAULTDET_blockIfFaultDetectedInTask () {
if (pxCurrentTCB->requiresFaultDetection) {
// if (instance->testedOnce) {
u8 taskId=(pxCurrentTCB->uxTaskNumber)-1;
FAULTDETECTOR_testpointShortDescriptorStr out;
do {
FAULTDETECTOR_getLastTestedPointShort(&FAULTDETECTOR_InstancePtr, taskId, &out);
}
#ifdef configENABLE_STRUCTS_SPECIFIC_MEMORY_OPTIMISATIONS_FOR_FAULT_DETECTOR
while((*((u32*)(&(lastRequestedTest[taskId]))))!=(*((u32*)(&out))));
// while(memcmp((void*) &(lastRequestedTest[taskId]), (void*) &out, sizeof(FAULTDETECTOR_testpointShortDescriptorStr))!=0);
#else
while (!(lastRequestedTest[taskId].checkId==out.checkId && lastRequestedTest[taskId].uniId==out.uniId && lastRequestedTest[taskId].executionId==out.executionId ));
#endif
#ifndef configOPTIMISATION_ON_FAULT_FAULT_DETECTOR_WRITES_PREVIOUS_EXECUTION_ID
if(FAULTDETECTOR_hasFault(&FAULTDETECTOR_InstancePtr, taskId)) {
#ifndef configIGNORE_FAULTS_DETECTED_BY_SW_FAULT_DETECTOR
while(1) {}
#endif
#endif
}
}
// }
}
#endif
#ifdef detectionPerformanceMeasurement
//used for performance measurements purpose
#define GOLDEN_RESULT_SIZE 64
int FAULTDET_testing_goldenResults_size=0;
int FAULTDET_testing_goldenResults_idx_tmp=0;
FAULTDETECTOR_testpointDescriptorStr FAULTDET_testing_goldenResults[GOLDEN_RESULT_SIZE];
void FAULTDET_testing_resetGoldens () {
FAULTDET_testing_goldenResults_size=0;
FAULTDET_testing_goldenResults_idx_tmp=0;
}
float FAULTDET_testing_relativeErrors[GOLDEN_RESULT_SIZE*FAULTDETECTOR_MAX_AOV_DIM];
int FAULTDET_testing_relativeErrors_size=0;
int FAULTDET_testing_total=0;
int FAULTDET_testing_ok=0;
int FAULTDET_testing_total_golden=0;
int FAULTDET_testing_ok_golden=0;
int FAULTDET_testing_falsePositives_golden=0;
int FAULTDET_testing_falseNegatives=0;
int FAULTDET_testing_noeffects=0;
int FAULTDET_testing_falseNegatives_wtolerance=0;
int FAULTDET_testing_ok_wtolerance=0;
char FAULTDET_testing_temp_aovchanged=0;
char FAULTDET_testing_temp_faultdetected=0;
char FAULTDET_testing_temp_lastoutputchanged=0;
void FAULTDET_testing_commitTmpStatsAndReset(u8 injectingFault) {
#ifndef csvOut
if (injectingFault) {
FAULTDET_testing_total++;
if (FAULTDET_testing_temp_aovchanged) {
if (FAULTDET_testing_temp_faultdetected) {
FAULTDET_testing_ok++;
FAULTDET_testing_ok_wtolerance++;
} else {
if (FAULTDET_testing_temp_lastoutputchanged) {
FAULTDET_testing_falseNegatives++;
if (FAULTDET_testing_relativeErrors[FAULTDET_testing_relativeErrors_size-1]>0.15f)
FAULTDET_testing_falseNegatives_wtolerance++;
else
FAULTDET_testing_ok_wtolerance++;
// for (int i=0; i<FAULTDET_testing_relativeErrors_size; i++) {
// xil_printf("%f;", FAULTDET_testing_relativeErrors[i]);
// }
// xil_printf("%f;", FAULTDET_testing_relativeErrors[FAULTDET_testing_relativeErrors_size-1]);
} else {
FAULTDET_testing_ok++;
FAULTDET_testing_ok_wtolerance++;
}
}
} else {
FAULTDET_testing_noeffects++;
}
} else {
FAULTDET_testing_total_golden++;
if (FAULTDET_testing_temp_faultdetected) {
FAULTDET_testing_falsePositives_golden++;
} else {
FAULTDET_testing_ok_golden++;
}
}
FAULTDET_testing_relativeErrors_size=0;
FAULTDET_testing_temp_faultdetected=0;
FAULTDET_testing_temp_aovchanged=0;
FAULTDET_testing_temp_lastoutputchanged=0;
#endif
}
int FAULTDET_testing_getTotal_golden() {
return FAULTDET_testing_total_golden;
}
int FAULTDET_testing_getOk_golden() {
return FAULTDET_testing_ok_golden;
}
int FAULTDET_testing_getTotal() {
return FAULTDET_testing_total;
}