__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;