if (((lr & 0x0F) == 1) || ((lr & 0x0F) == 9))
    {
        CommonProcessingException(&sSave, msp);
    }
    else if ((lr & 0x0F) == 0x0D)
    {
        CommonProcessingException(&sSave, psp);
    }

HardFault_Handler\
                PROC
                EXPORT  HardFault_Handler          [WEAK]
                TST LR, #4
                ITE EQ
                MRSEQ R0, MSP
                MRSNE R0, PSP
                B hard_fault_handler_c
                ENDP

void hard_fault_handler_c (unsigned int * hardfault_args)
{
  __IO unsigned int stack_ptr;
  __IO unsigned int stacked_r0;
  __IO unsigned int stacked_r1;
  __IO unsigned int stacked_r2;
  __IO unsigned int stacked_r3;
  __IO unsigned int stacked_r12;
  __IO unsigned int stacked_lr;
  __IO unsigned int stacked_pc;
  __IO unsigned int stacked_psr;

  stack_ptr = ((unsigned long) hardfault_args);
  stacked_r0 = ((unsigned long) hardfault_args[0]);
  stacked_r1 = ((unsigned long) hardfault_args[1]);
  stacked_r2 = ((unsigned long) hardfault_args[2]);
  stacked_r3 = ((unsigned long) hardfault_args[3]);

  stacked_r12 = ((unsigned long) hardfault_args[4]);
  stacked_lr = ((unsigned long) hardfault_args[5]);
  stacked_pc = ((unsigned long) hardfault_args[6]);
  stacked_psr = ((unsigned long) hardfault_args[7]);

  printf ("\n\n[Hard fault handler - all numbers in hex]\n");
  printf ("stack_ptr = %x\n", stack_ptr);
  printf ("R0 = %x\n", stacked_r0);
  printf ("R1 = %x\n", stacked_r1);
  printf ("R2 = %x\n", stacked_r2);
  printf ("R3 = %x\n", stacked_r3);
  printf ("R12 = %x\n", stacked_r12);
  printf ("LR [R14] = %x  subroutine call return address\n", stacked_lr);
  printf ("PC [R15] = %x  program counter\n", stacked_pc);
  printf ("PSR = %x\n", stacked_psr);
  printf ("BFAR = 0x%x\n", (uint32_t)(*((volatile unsigned long *)(0xE000ED38))));
  printf ("CFSR = 0x%x\n", (uint32_t)(*((volatile unsigned long *)(0xE000ED28))));
  printf ("HFSR = 0x%x\n", (uint32_t)(*((volatile unsigned long *)(0xE000ED2C))));
  printf ("DFSR = 0x%x\n", (uint32_t)(*((volatile unsigned long *)(0xE000ED30))));
  printf ("AFSR = 0x%x\n", (uint32_t)(*((volatile unsigned long *)(0xE000ED3C))));
  printf ("SCB_SHCSR = 0x%x\n", SCB->SHCSR);
  memstat ();

  while (1);
}

__asm void xPortPendSVHandler( void )
{
    extern uxCriticalNesting;
    extern pxCurrentTCB;
    extern vTaskSwitchContext;

    PRESERVE8

    mrs r0, psp

    /* Get the location of the current TCB. */
    ldr    r3, =pxCurrentTCB
    ldr    r2, [r3]

    /* Is the task using the FPU context?  If so, push high vfp registers. */
    tst r14, #0x10
    it eq
    vstmdbeq r0!, {s16-s31}

    /* Save the core registers. */
    stmdb r0!, {r4-r11, r14}

    /* Save the new top of stack into the first member of the TCB. */
    str r0, [r2]

    stmdb sp!, {r3}
    mov r0, #configMAX_SYSCALL_INTERRUPT_PRIORITY
    msr basepri, r0
    bl vTaskSwitchContext
    mov r0, #0
    msr basepri, r0
    ldmia sp!, {r3}

    /* The first item in pxCurrentTCB is the task top of stack. */
    ldr r1, [r3]
    ldr r0, [r1]

    /* Pop the core registers. */
    ldmia r0!, {r4-r11, r14}

    /* Is the task using the FPU context?  If so, pop the high vfp registers
    too. */
    tst r14, #0x10
    it eq
    vldmiaeq r0!, {s16-s31}

    msr psp, r0

    #ifdef WORKAROUND_PMU_CM001 /* XMC4000 specific errata */
        #if WORKAROUND_PMU_CM001 == 1
            push { r14 }
            pop { pc }
        #endif
    #endif

    bx r14
    nop
  nop
}