;/*
; * Copyright (c) 2006-2018, RT-Thread Development Team
; *
; * SPDX-License-Identifier: Apache-2.0
; *
; * Change Logs:
; * Date           Author       Notes
; * 2009-01-17     Bernard      first version
; * 2009-09-27     Bernard      add protect when contex switch occurs
; * 2012-01-01     aozima       support context switch load/store FPU register.
; * 2013-06-18     aozima       add restore MSP feature.
; * 2013-06-23     aozima       support lazy stack optimized.
; * 2018-07-24     aozima       enhancement hard fault exception handler.
; */

;/**
; * @addtogroup cortex-m4
; */
;/*@{*/

SCB_VTOR        EQU     0xE000ED08               ; Vector Table Offset Register
NVIC_INT_CTRL   EQU     0xE000ED04               ; interrupt control state register
NVIC_SYSPRI2    EQU     0xE000ED20               ; system priority register (2)
NVIC_PENDSV_PRI EQU     0xFFFF0000               ; PendSV and SysTick priority value (lowest)
NVIC_PENDSVSET  EQU     0x10000000               ; value to trigger PendSV exception

    SECTION    .text:CODE(2)
    THUMB
    REQUIRE8
    PRESERVE8
    
    IMPORT rt_hw_hard_fault_exception
    EXPORT HardFault_Handler
HardFault_Handler:

    ; get current context
    MRS     r0, msp                 ; get fault context from handler.
    TST     lr, #0x04               ; if(!EXC_RETURN[2])
    BEQ     _get_sp_done
    MRS     r0, psp                 ; get fault context from thread.
_get_sp_done

    STMFD   r0!, {r4 - r11}         ; push r4 - r11 register
    ;STMFD   r0!, {lr}               ; push exec_return register
#if defined ( __ARMVFP__ )
    SUB     r0, r0, #0x04           ; push dummy for flag
    STR     lr, [r0]
#endif
    SUB     r0, r0, #0x04
    STR     lr, [r0]

    TST     lr, #0x04               ; if(!EXC_RETURN[2])
    BEQ     _update_msp
    MSR     psp, r0                 ; update stack pointer to PSP.
    B       _update_done
_update_msp
    MSR     msp, r0                 ; update stack pointer to MSP.
_update_done

    PUSH    {lr}
    BL      rt_hw_hard_fault_exception
    POP     {lr}

    ORR     lr, lr, #0x04
    BX      lr
    
    IMPORT SVC_Handler
    IMPORT svc_exception 
    EXPORT SVC_Handler_asm
SVC_Handler_asm:
        ; get current context
    TST     lr, #0x04               ; if(!EXC_RETURN[2])
    ITE     EQ
    MRSEQ   r0, msp     ; r0保存msp               
    MRSNE   r0, psp     ; r0保存psp
    
    ldr    r1, [r0, #6*4] ;get pc ， r0加6个寄存器就是lr即exec_return得到执行异常后需要跳到哪里执行的addr
    sub    r1, r1, #2 ; addr减2的指令就是我们设置的断点的指令即 swi # 指令
    ldrh   r2, [r1] ; 获取到swi指令, 0xdfxx
    
    ands   r2, r2, #0x0ff ; 判断swi指令的低八位是不是0
    
    BEQ    SVC_Handler ; 如果后swi指令的后八位是0，跳转到原来的freertos SVC处理函数，0xdf00
    
    ; 自己的代码，处理断点
    STMFD  r0!, {r4 - r11} ; 入栈r4 - r11
    STMFD  r0!, {lr}       ; 入栈 EXC_RETURN
    
    TST    lr, #0x04 ; 判断异常时用的是msp还是psp
    ITE    EQ
    
    MSREQ  msp, r0 ; r0保存msp              
    MSRNE  psp, r0 ; r0保存psp
    
    PUSH   {lr}
    BL     svc_exception ; r0保存了sp
    POP    {lr}
        
    ORR    lr, lr, #0x04
    BX     lr
    
    END