__asm void SVC_Handler (void)
{
movs R0, #4
mov R1, LR
tst R0, R1
beq msp_stacked

psp_stacked
mrs R0,PSP // Read PSP
b done

msp_stacked
mrs R0,MSP // Read MSP

done
ldr R1,[R0,#24] // Read Saved PC from Stack
subs R1,R1,#2 // Point to SVC Instruction
ldrb R1,[R1] // Load SVC index
cmp R1,#0
beq svc_0 // SVC0
cmp R1,#1
beq svc_1 // SVC1
cmp R1,#2
beq svc_2 // SVC2

svc_0
ldr R0, =__cpp(&tn_start_first_task)
bx R0

svc_1
ldr R0, =__cpp(&tn_tick_int_processing)
bx R0

svc_2
ldr R0, =__cpp(&tn_timer)
bx R0

ALIGN
}

.global HardFaultException
.extern print_fault
HardFaultException:
mov r0, sp
push {lr}
bl print_fault
pop {pc}

void print_fault(sFAULT *fault)
{
con_str("FAULT:\n\r");
con_str("r0 = "); con_dword(fault->r0); con_str(", ");
con_str("r1 = "); con_dword(fault->r1); con_str(", ");
con_str("r2 = "); con_dword(fault->r2); con_str(", ");
con_str("r3 = "); con_dword(fault->r3); con_str("\n\r");
con_str("r12 = "); con_dword(fault->r12); con_str(", ");
con_str("lr = "); con_dword(fault->lr); con_str(", ");
con_str("pc = "); con_dword(fault->pc); con_str(", ");
con_str("xpsr = "); con_dword(fault->xpsr); con_str("\n\r");
con_start();
}

typedef struct sFAULT
{
DWORD r0; //!< Регистр R0
DWORD r1; //!< Регистр R1
DWORD r2; //!< Регистр R2
DWORD r3; //!< Регистр R3
DWORD r12; //!< Регистр R12
DWORD lr; //!< Регистр LR
DWORD pc; //!< Регистр PC
DWORD xpsr; //!< Регистр XPSR
} sFAULT;

__stackless void FaultHandler(void * p_base, unsigned long EXC_RETURN)
{
SaveContext(p_base, EXC_RETURN);
EXPT.VERSION = APP_Get_FwVersion();
gMarker = EXCEPTION_MARKER;
#ifdef NDEBUG
SYS_Reset();
#else
while (1);
#endif
}

__stackless __irq void HardFaultHandler(void)
{
if (SCB->HFSR & SCB_HFSR_DEBUGEVT_Pos)
return;
FaultHandler((void*)( (__get_LR() & BIT(2)) ? __get_PSP() : __get_MSP()),
__get_LR());
}

__stackless __irq void MemoryFaultHandler(void)
{
FaultHandler((void*)( (__get_LR() & BIT(2)) ? __get_PSP() : __get_MSP()),
__get_LR());
}

__stackless __irq void BusFaultHandler(void)
{
FaultHandler((void*)( (__get_LR() & BIT(2)) ? __get_PSP() : __get_MSP()),
__get_LR());

}

__stackless __irq void UsageFaultHandler(void)
{
FaultHandler((void*)( (__get_LR() & BIT(2)) ? __get_PSP() : __get_MSP()),
__get_LR());

// while(1);
}


void SaveContext(void * p_base, unsigned long EXC_RETURN)
{
EXPT.Registers = *(struct exception_saved_context * )p_base;
EXPT.SP = (DWORD)p_base + sizeof(struct exception_saved_context);

EXPT.Status.dwRaw = SCB->CFSR;
EXPT.Exception = (int_source_t)(SCB->ICSR & SCB_ICSR_VECTACTIVE_Msk);
EXPT.EXC_RETURN = ( EXPT.Status.BFSR.BFARVALID) ? SCB->BFAR : EXC_RETURN;
SCB->CFSR = 0;

EXPT.HeapFree = OS_MEMORY_AVAILABLE();
EXPT.ShedulerState = OS_GET_SHEDULER_STATE();
EXPT.Context.StackFree = OS_GET_TASK_STACK_WATERMARK(OS_GET_CURRENT_TASK_HANDLE());
strncpy(EXPT.Context.Name, (char*)OS_GET_CURRENT_TASK_NAME() , sizeof(EXPT.Context.Name));
EXPT.Context.Name[sizeof(EXPT.Context.Name)-1] = '\0';

11e:    1f 92           push    r1
120:    0f 92           push    r0
122:    0f b6           in    r0, 0x3f; 63
124:    0f 92           push    r0
126:    11 24           eor    r1, r1
128:    2f 93           push    r18
12a:    3f 93           push    r19
12c:    4f 93           push    r20
12e:    5f 93           push    r21
130:    6f 93           push    r22
132:    7f 93           push    r23
134:    8f 93           push    r24
136:    9f 93           push    r25
138:    af 93           push    r26
13a:    bf 93           push    r27
13c:    ef 93           push    r30
13e:    ff 93           push    r31