Permalink
Cannot retrieve contributors at this time
Name already in use
A tag already exists with the provided branch name. Many Git commands accept both tag and branch names, so creating this branch may cause unexpected behavior. Are you sure you want to create this branch?
MMD_IAR/JLINK_MONITOR_ISR_IAR.s
Go to fileThis commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
917 lines (870 sloc)
39.7 KB
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
/********************************************************************* | |
* SEGGER Microcontroller GmbH & Co. KG * | |
* The Embedded Experts * | |
********************************************************************** | |
* * | |
* (c) 1995 - 2015 SEGGER Microcontroller GmbH & Co. KG * | |
* * | |
* www.segger.com Support: support@segger.com * | |
* * | |
********************************************************************** | |
---------------------------------------------------------------------- | |
File : JLINK_MONITOR_ISR_SES.s | |
Purpose : Implementation of debug monitor for J-Link monitor mode | |
debug on Cortex-M devices, supporting SES compiler. | |
-------- END-OF-HEADER --------------------------------------------- | |
*/ | |
// .name JLINK_MONITOR_ISR | |
NAME JLINK_MONITOR_ISR | |
// .syntax unified | |
// .extern JLINK_MONITOR_OnEnter | |
EXTERN JLINK_MONITOR_OnEnter | |
// .extern JLINK_MONITOR_OnExit | |
EXTERN JLINK_MONITOR_OnExit | |
// .extern JLINK_MONITOR_OnPoll | |
EXTERN JLINK_MONITOR_OnPoll | |
// .global DebugMon_Handler | |
EXPORT DebugMon_Handler | |
/********************************************************************* | |
* | |
* Defines, configurable | |
* | |
********************************************************************** | |
*/ | |
#define _MON_VERSION 100 // V x.yy | |
/********************************************************************* | |
* | |
* Defines, fixed | |
* | |
********************************************************************** | |
*/ | |
#define _APP_SP_OFF_R0 0x00 | |
#define _APP_SP_OFF_R1 0x04 | |
#define _APP_SP_OFF_R2 0x08 | |
#define _APP_SP_OFF_R3 0x0C | |
#define _APP_SP_OFF_R12 0x10 | |
#define _APP_SP_OFF_R14_LR 0x14 | |
#define _APP_SP_OFF_PC 0x18 | |
#define _APP_SP_OFF_XPSR 0x1C | |
#define _APP_SP_OFF_S0 0x20 | |
#define _APP_SP_OFF_S1 0x24 | |
#define _APP_SP_OFF_S2 0x28 | |
#define _APP_SP_OFF_S3 0x2C | |
#define _APP_SP_OFF_S4 0x30 | |
#define _APP_SP_OFF_S5 0x34 | |
#define _APP_SP_OFF_S6 0x38 | |
#define _APP_SP_OFF_S7 0x3C | |
#define _APP_SP_OFF_S8 0x40 | |
#define _APP_SP_OFF_S9 0x44 | |
#define _APP_SP_OFF_S10 0x48 | |
#define _APP_SP_OFF_S11 0x4C | |
#define _APP_SP_OFF_S12 0x50 | |
#define _APP_SP_OFF_S13 0x54 | |
#define _APP_SP_OFF_S14 0x58 | |
#define _APP_SP_OFF_S15 0x5C | |
#define _APP_SP_OFF_FPSCR 0x60 | |
#define _NUM_BYTES_BASIC_STACKFRAME 32 | |
#define _NUM_BYTES_EXTENDED_STACKFRAME 72 | |
#define _SYSTEM_DCRDR_OFF 0x00 | |
#define _SYSTEM_DEMCR_OFF 0x04 | |
#define _SYSTEM_DHCSR 0xE000EDF0 // Debug Halting Control and Status Register (DHCSR) | |
#define _SYSTEM_DCRSR 0xE000EDF4 // Debug Core Register Selector Register (DCRSR) | |
#define _SYSTEM_DCRDR 0xE000EDF8 // Debug Core Register Data Register (DCRDR) | |
#define _SYSTEM_DEMCR 0xE000EDFC // Debug Exception and Monitor Control Register (DEMCR) | |
#define _SYSTEM_FPCCR 0xE000EF34 // Floating-Point Context Control Register (FPCCR) | |
#define _SYSTEM_FPCAR 0xE000EF38 // Floating-Point Context Address Register (FPCAR) | |
#define _SYSTEM_FPDSCR 0xE000EF3C // Floating-Point Default Status Control Register (FPDSCR) | |
#define _SYSTEM_MVFR0 0xE000EF40 // Media and FP Feature Register 0 (MVFR0) | |
#define _SYSTEM_MVFR1 0xE000EF44 // Media and FP Feature Register 1 (MVFR1) | |
/* | |
* Defines for determining if the current debug config supports FPU registers | |
* For some compilers like IAR EWARM when disabling the FPU in the compiler settings an error is thrown when | |
*/ | |
#ifdef __FPU_PRESENT | |
#if __FPU_PRESENT | |
#define _HAS_FPU_REGS 1 | |
#else | |
#define _HAS_FPU_REGS 0 | |
#endif | |
#else | |
#define _HAS_FPU_REGS 0 | |
#endif | |
/********************************************************************* | |
* | |
* Signature of monitor | |
* | |
* Function description | |
* Needed for targets where also a boot ROM is present that possibly specifies a vector table with a valid debug monitor exception entry | |
*/ | |
// .section .text, "ax" | |
RSEG `.text`:CODE | |
// | |
// JLINKMONHANDLER | |
// | |
// .byte 0x4A | |
DC8 0x4A | |
// .byte 0x4C | |
DC8 0x4C | |
// .byte 0x49 | |
DC8 0x49 | |
// .byte 0x4E | |
DC8 0x4E | |
// .byte 0x4B | |
DC8 0x4B | |
// .byte 0x4D | |
DC8 0x4D | |
// .byte 0x4F | |
DC8 0x4F | |
// .byte 0x4E | |
DC8 0x4E | |
// .byte 0x48 | |
DC8 0x48 | |
// .byte 0x41 | |
DC8 0x41 | |
// .byte 0x4E | |
DC8 0x4E | |
// .byte 0x44 | |
DC8 0x44 | |
// .byte 0x4C | |
DC8 0x4C | |
// .byte 0x45 | |
DC8 0x45 | |
// .byte 0x52 | |
DC8 0x52 | |
// .byte 0x00 // Align to 8-bytes | |
DC8 0x00 | |
/********************************************************************* | |
* | |
* DebugMon_Handler() | |
* | |
* Function description | |
* Debug monitor handler. CPU enters this handler in case a "halt" request is made from the debugger. | |
* This handler is also responsible for handling commands that are sent by the debugger. | |
* | |
* Notes | |
* This is actually the ISR for the debug inerrupt (exception no. 12) | |
*/ | |
// .thumb_func | |
THUMB | |
DebugMon_Handler: | |
/* | |
General procedure: | |
DCRDR is used as communication register | |
DEMCR[19] is used as ready flag | |
For the command J-Link sends to the monitor: DCRDR[7:0] == Cmd, DCRDR[31:8] == ParamData | |
1) Monitor sets DEMCR[19] whenever it is ready to receive new commands/data | |
DEMCR[19] is initially set on debug monitor entry | |
2) J-Link will clear once it has placed conmmand/data in DCRDR for J-Link | |
3) Monitor will wait for DEMCR[19] to be cleared | |
4) Monitor will process command (May cause additional data transfers etc., depends on command | |
5) No restart-CPU command? => Back to 2), Otherwise => 6) | |
6) Monitor will clear DEMCR[19] 19 to indicate that it is no longer ready | |
*/ | |
PUSH {LR} | |
BL JLINK_MONITOR_OnEnter | |
POP {LR} | |
LDR.N R3,_AddrDCRDR // 0xe000edf8 == _SYSTEM_DCRDR | |
B.N _IndicateMonReady | |
_WaitProbeReadIndicateMonRdy: // while(_SYSTEM_DEMCR & (1uL << 19)); => Wait until J-Link has read item | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR | |
LSLS R0,R0,#+12 | |
BMI.N _WaitProbeReadIndicateMonRdy | |
_IndicateMonReady: | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Set MON_REQ bit, so J-Link knows monitor is ready to receive commands | |
ORR R0,R0,#0x80000 | |
STR R0,[R3, #+_SYSTEM_DEMCR_OFF] | |
/* | |
During command loop: | |
R0 = Tmp | |
R1 = Tmp | |
R2 = Tmp | |
R3 = &_SYSTEM_DCRDR (allows also access to DEMCR with offset) | |
R12 = Tmp | |
Outside command loop R0-R3 and R12 may be overwritten by MONITOR_OnPoll() | |
*/ | |
_WaitForJLinkCmd: // do { | |
PUSH {LR} | |
BL JLINK_MONITOR_OnPoll | |
POP {LR} | |
LDR.N R3,_AddrDCRDR // 0xe000edf8 == _SYSTEM_DCRDR | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] | |
LSRS R0,R0,#+20 // DEMCR[19] -> Carry Clear? => J-Link has placed command for us | |
BCS _WaitForJLinkCmd | |
/* | |
Perform command | |
Command is placed by J-Link in DCRDR[7:0] and additional parameter data is stored in DCRDR[31:8] | |
J-Link clears DEMCR[19] to indicate that it placed a command/data or read data | |
Monitor sets DEMCR[19] to indicate that it placed data or read data / is ready for a new command | |
Setting DEMCR[19] indicates "monitor ready for new command / data" and also indicates: "data has been placed in DCRDR by monitor, for J-Link" | |
Therefore it is responsibility of the commands to respond to the commands accordingly | |
Commands for debug monitor | |
Commands must not exceed 0xFF (255) as we only defined 8-bits for command-part. Higher 24-bits are parameter info for current command | |
Protocol for different commands: | |
J-Link: Cmd -> DCRDR, DEMCR[19] -> 0 => Cmd placed by probe | |
*/ | |
LDR R0,[R3, #+_SYSTEM_DCRDR_OFF] // ParamInfo = _SYSTEM_DCRDR | |
LSRS R1,R0,#+8 // ParamInfo >>= 8 | |
LSLS R0,R0,#+24 | |
LSRS R0,R0,#+24 // Cmd = ParamInfo & 0xFF | |
// | |
// switch (Cmd) | |
// | |
CMP R0,#+0 | |
BEQ.N _HandleGetMonVersion // case _MON_CMD_GET_MONITOR_VERSION | |
CMP R0,#+2 | |
BEQ.N _HandleReadReg // case _MON_CMD_READ_REG | |
BCC.N _HandleRestartCPU // case _MON_CMD_RESTART_CPU | |
CMP R0,#+3 | |
BEQ.N _HandleWriteReg_Veneer // case _MON_CMD_WRITE_REG | |
B.N _IndicateMonReady // default : while (1); | |
/* | |
Return | |
_MON_CMD_RESTART_CPU | |
CPU: DEMCR[19] -> 0 => Monitor no longer ready | |
*/ | |
_HandleRestartCPU: | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR &= ~(1uL << 19); => Clear MON_REQ to indicate that monitor is no longer active | |
BIC R0,R0,#0x80000 | |
STR R0,[R3, #+_SYSTEM_DEMCR_OFF] | |
PUSH {LR} | |
BL JLINK_MONITOR_OnExit | |
POP {PC} | |
// | |
// Place data section here to not get in trouble with load-offsets | |
// | |
// .section .text, "ax", %progbits | |
RSEG `.text`:DATA (2) | |
// .align 2 | |
_AddrDCRDR: | |
// .long 0xE000EDF8 | |
DC32 0xE000EDF8 | |
_AddrCPACR: | |
// .long 0xE000ED88 | |
DC32 0xE000ED88 | |
// .section .text, "ax" | |
RSEG `.text`:CODE | |
// .thumb_func | |
THUMB | |
;/********************************************************************* | |
;* | |
;* _HandleGetMonVersion | |
;* | |
;*/ | |
_HandleGetMonVersion: | |
/* | |
_MON_CMD_GET_MONITOR_VERSION | |
CPU: Data -> DCRDR, DEMCR[19] -> 1 => Data ready | |
J-Link: DCRDR -> Read, DEMCR[19] -> 0 => Data read | |
CPU: DEMCR[19] -> 1 => Mon ready | |
*/ | |
MOVS R0,#+_MON_VERSION | |
STR R0,[R3, #+_SYSTEM_DCRDR_OFF] // _SYSTEM_DCRDR = x | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Set MON_REQ bit, so J-Link knows monitor is ready to receive commands | |
ORR R0,R0,#0x80000 | |
STR R0,[R3, #+_SYSTEM_DEMCR_OFF] // Indicate data ready | |
B _WaitProbeReadIndicateMonRdy | |
/********************************************************************* | |
* | |
* _HandleReadReg | |
* | |
*/ | |
_HandleWriteReg_Veneer: | |
B.N _HandleWriteReg | |
_HandleReadReg: | |
/* | |
_MON_CMD_READ_REG | |
CPU: Data -> DCRDR, DEMCR[19] -> 1 => Data ready | |
J-Link: DCRDR -> Read, DEMCR[19] -> 0 => Data read | |
CPU: DEMCR[19] -> 1 => Mon ready | |
Register indexes | |
0-15: R0-R15 (13 == R13 reserved => is banked ... Has to be read as PSP / MSP. Decision has to be done by J-Link DLL side!) | |
16: XPSR | |
17: MSP | |
18: PSP | |
19: CFBP CONTROL/FAULTMASK/BASEPRI/PRIMASK (packed into 4 bytes of word. CONTROL = CFBP[31:24], FAULTMASK = CFBP[16:23], BASEPRI = CFBP[15:8], PRIMASK = CFBP[7:0] | |
20: FPSCR | |
21-52: FPS0-FPS31 | |
Register usage when entering this "subroutine": | |
R0 Cmd | |
R1 ParamInfo | |
R2 --- | |
R3 = &_SYSTEM_DCRDR (allows also access to DEMCR with offset) | |
R12 --- | |
Table B1-9 EXC_RETURN definition of exception return behavior, with FP extension | |
LR Return to Return SP Frame type | |
--------------------------------------------------------- | |
0xFFFFFFE1 Handler mode. MSP Extended | |
0xFFFFFFE9 Thread mode MSP Extended | |
0xFFFFFFED Thread mode PSP Extended | |
0xFFFFFFF1 Handler mode. MSP Basic | |
0xFFFFFFF9 Thread mode MSP Basic | |
0xFFFFFFFD Thread mode PSP Basic | |
So LR[2] == 1 => Return stack == PSP else MSP | |
R0-R3, R12, PC, xPSR can be read from application stackpointer | |
Other regs can be read directly | |
*/ | |
LSRS R2,LR,#+3 // Shift LR[2] into carry => Carry clear means that CPU was running on MSP | |
ITE CS | |
MRSCS R2,PSP | |
MRSCC R2,MSP | |
CMP R1,#+4 // if (RegIndex < 4) { (R0-R3) | |
BCS _HandleReadRegR4 | |
LDR R0,[R2, R1, LSL #+2] // v = [SP + Rx * 4] (R0-R3) | |
B.N _HandleReadRegDone | |
_HandleReadRegR4: | |
CMP R1,#+5 // if (RegIndex < 5) { (R4) | |
BCS _HandleReadRegR5 | |
MOV R0,R4 | |
B.N _HandleReadRegDone | |
_HandleReadRegR5: | |
CMP R1,#+6 // if (RegIndex < 6) { (R5) | |
BCS _HandleReadRegR6 | |
MOV R0,R5 | |
B.N _HandleReadRegDone | |
_HandleReadRegR6: | |
CMP R1,#+7 // if (RegIndex < 7) { (R6) | |
BCS _HandleReadRegR7 | |
MOV R0,R6 | |
B.N _HandleReadRegDone | |
_HandleReadRegR7: | |
CMP R1,#+8 // if (RegIndex < 8) { (R7) | |
BCS _HandleReadRegR8 | |
MOV R0,R7 | |
B.N _HandleReadRegDone | |
_HandleReadRegR8: | |
CMP R1,#+9 // if (RegIndex < 9) { (R8) | |
BCS _HandleReadRegR9 | |
MOV R0,R8 | |
B.N _HandleReadRegDone | |
_HandleReadRegR9: | |
CMP R1,#+10 // if (RegIndex < 10) { (R9) | |
BCS _HandleReadRegR10 | |
MOV R0,R9 | |
B.N _HandleReadRegDone | |
_HandleReadRegR10: | |
CMP R1,#+11 // if (RegIndex < 11) { (R10) | |
BCS _HandleReadRegR11 | |
MOV R0,R10 | |
B.N _HandleReadRegDone | |
_HandleReadRegR11: | |
CMP R1,#+12 // if (RegIndex < 12) { (R11) | |
BCS _HandleReadRegR12 | |
MOV R0,R11 | |
B.N _HandleReadRegDone | |
_HandleReadRegR12: | |
CMP R1,#+14 // if (RegIndex < 14) { (R12) | |
BCS _HandleReadRegR14 | |
LDR R0,[R2, #+_APP_SP_OFF_R12] | |
B.N _HandleReadRegDone | |
_HandleReadRegR14: | |
CMP R1,#+15 // if (RegIndex < 15) { (R14 / LR) | |
BCS _HandleReadRegR15 | |
LDR R0,[R2, #+_APP_SP_OFF_R14_LR] | |
B.N _HandleReadRegDone | |
_HandleReadRegR15: | |
CMP R1,#+16 // if (RegIndex < 16) { (R15 / PC) | |
BCS _HandleReadRegXPSR | |
LDR R0,[R2, #+_APP_SP_OFF_PC] | |
B.N _HandleReadRegDone | |
_HandleReadRegXPSR: | |
CMP R1,#+17 // if (RegIndex < 17) { (xPSR) | |
BCS _HandleReadRegMSP | |
LDR R0,[R2, #+_APP_SP_OFF_XPSR] | |
B.N _HandleReadRegDone | |
_HandleReadRegMSP: | |
/* | |
Stackpointer is tricky because we need to get some info about the SP used in the user app, first | |
Handle reading R0-R3 which can be read right from application stackpointer | |
Table B1-9 EXC_RETURN definition of exception return behavior, with FP extension | |
LR Return to Return SP Frame type | |
--------------------------------------------------------- | |
0xFFFFFFE1 Handler mode. MSP Extended | |
0xFFFFFFE9 Thread mode MSP Extended | |
0xFFFFFFED Thread mode PSP Extended | |
0xFFFFFFF1 Handler mode. MSP Basic | |
0xFFFFFFF9 Thread mode MSP Basic | |
0xFFFFFFFD Thread mode PSP Basic | |
So LR[2] == 1 => Return stack == PSP else MSP | |
Per architecture definition: Inside monitor (exception) SP = MSP | |
Stack pointer handling is complicated because it is different what is pushed on the stack before entering the monitor ISR... | |
Cortex-M: 8 regs | |
Cortex-M + forced-stack-alignment: 8 regs + 1 dummy-word if stack was not 8-byte aligned | |
Cortex-M + FPU: 8 regs + 17 FPU regs + 1 dummy-word + 1-dummy word if stack was not 8-byte aligned | |
Cortex-M + FPU + lazy mode: 8 regs + 17 dummy-words + 1 dummy-word + 1-dummy word if stack was not 8-byte aligned | |
*/ | |
CMP R1,#+18 // if (RegIndex < 18) { (MSP) | |
BCS _HandleReadRegPSP | |
MRS R0,MSP | |
LSRS R1,LR,#+3 // LR[2] -> Carry == 0 => CPU was running on MSP => Needs correction | |
BCS _HandleReadRegDone_Veneer // CPU was running on PSP? => No correction necessary | |
_HandleSPCorrection: | |
LSRS R1,LR,#+5 // LR[4] -> Carry == 0 => extended stack frame has been allocated. See ARM DDI0403D, B1.5.7 Stack alignment on exception entry | |
ITE CS | |
ADDCS R0,R0,#+_NUM_BYTES_BASIC_STACKFRAME | |
ADDCC R0,R0,#+_NUM_BYTES_EXTENDED_STACKFRAME | |
LDR R1,[R2, #+_APP_SP_OFF_XPSR] // Get xPSR from application stack (R2 has been set to app stack on beginning of _HandleReadReg) | |
LSRS R1,R1,#+5 // xPSR[9] -> Carry == 1 => Stack has been force-aligned before pushing regs. See ARM DDI0403D, B1.5.7 Stack alignment on exception entry | |
IT CS | |
ADDCS R0,R0,#+4 | |
B _HandleReadRegDone | |
_HandleReadRegPSP: // RegIndex == 18 | |
CMP R1,#+19 // if (RegIndex < 19) { | |
BCS _HandleReadRegCFBP | |
MRS R0,PSP // PSP is not touched by monitor | |
LSRS R1,LR,#+3 // LR[2] -> Carry == 1 => CPU was running on PSP => Needs correction | |
BCC _HandleReadRegDone_Veneer // CPU was running on MSP? => No correction of PSP necessary | |
B _HandleSPCorrection | |
_HandleReadRegCFBP: | |
/* | |
CFBP is a register that can only be read via debug probe and is a merger of the following regs: | |
CONTROL/FAULTMASK/BASEPRI/PRIMASK (packed into 4 bytes of word. CONTROL = CFBP[31:24], FAULTMASK = CFBP[16:23], BASEPRI = CFBP[15:8], PRIMASK = CFBP[7:0] | |
To keep J-Link side the same for monitor and halt mode, we also return CFBP in monitor mode | |
*/ | |
CMP R1,#+20 // if (RegIndex < 20) { (CFBP) | |
BCS _HandleReadRegFPU | |
MOVS R0,#+0 | |
MRS R2,PRIMASK | |
ORRS R0,R2 // Merge PRIMASK into CFBP[7:0] | |
MRS R2,BASEPRI | |
LSLS R2,R2,#+8 // Merge BASEPRI into CFBP[15:8] | |
ORRS R0,R2 | |
MRS R2,FAULTMASK | |
LSLS R2,R2,#+16 // Merge FAULTMASK into CFBP[23:16] | |
ORRS R0,R2 | |
MRS R2,CONTROL | |
LSRS R1,LR,#3 // LR[2] -> Carry. CONTROL.SPSEL is saved to LR[2] on exception entry => ARM DDI0403D, B1.5.6 Exception entry behavior | |
IT CS // As J-Link sees value of CONTROL at application time, we need reconstruct original value of CONTROL | |
ORRCS R2,R2,#+2 // CONTROL.SPSEL (CONTROL[1]) == 0 inside monitor | |
LSRS R1,LR,#+5 // LR[4] == NOT(CONTROL.FPCA) -> Carry | |
ITE CS // Merge original value of FPCA (CONTROL[2]) into read data | |
BICCS R2,R2,#+0x04 // Remember LR contains NOT(CONTROL) | |
ORRCC R2,R2,#+0x04 | |
LSLS R2,R2,#+24 | |
ORRS R0,R2 | |
B.N _HandleReadRegDone | |
_HandleReadRegFPU: | |
#if _HAS_FPU_REGS | |
CMP R1,#+53 // if (RegIndex < 53) { (20 (FPSCR), 21-52 FPS0-FPS31) | |
BCS _HandleReadRegDone_Veneer | |
/* | |
Read Coprocessor Access Control Register (CPACR) to check if CP10 and CP11 are enabled | |
If not, access to floating point is not possible | |
CPACR[21:20] == CP10 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved | |
CPACR[23:22] == CP11 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved | |
*/ | |
LDR R0,_AddrCPACR | |
LDR R0,[R0] | |
LSLS R0,R0,#+8 | |
LSRS R0,R0,#+28 | |
CMP R0,#+0xF | |
BEQ _HandleReadRegFPU_Allowed | |
CMP R0,#+0x5 | |
BNE _HandleReadRegDone_Veneer | |
_HandleReadRegFPU_Allowed: | |
CMP R1,#+21 // if (RegIndex < 21) (20 == FPSCR) | |
BCS _HandleReadRegFPS0_FPS31 | |
LSRS R0,LR,#+5 // CONTROL[2] == FPCA => NOT(FPCA) saved to LR[4]. LR[4] == 0 => Extended stack frame, so FPU regs possibly on stack | |
BCS _HandleReadFPSCRLazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame | |
LDR R0,=_SYSTEM_FPCCR | |
LDR R0,[R0] | |
LSLS R0,R0,#+2 // FPCCR[30] -> Carry == 1 indicates if lazy mode is active, so space on stack is reserved but FPU registers are not saved on stack | |
BCS _HandleReadFPSCRLazyMode | |
LDR R0,[R2, #+_APP_SP_OFF_FPSCR] | |
B _HandleReadRegDone | |
_HandleReadFPSCRLazyMode: | |
VMRS R0,FPSCR | |
B _HandleReadRegDone | |
_HandleReadRegFPS0_FPS31: // RegIndex == 21-52 | |
LSRS R0,LR,#+5 // CONTROL[2] == FPCA => NOT(FPCA) saved to LR[4]. LR[4] == 0 => Extended stack frame, so FPU regs possibly on stack | |
BCS _HandleReadFPS0_FPS31LazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame | |
LDR R0,=_SYSTEM_FPCCR | |
LDR R0,[R0] | |
LSLS R0,R0,#+2 // FPCCR[30] -> Carry == 1 indicates if lazy mode is active, so space on stack is reserved but FPU registers are not saved on stack | |
BCS _HandleReadFPS0_FPS31LazyMode | |
SUBS R1,#+21 // Convert absolute reg index into rel. one | |
LSLS R1,R1,#+2 // RegIndex to position on stack | |
ADDS R1,#+_APP_SP_OFF_S0 | |
LDR R0,[R2, R1] | |
_HandleReadRegDone_Veneer: | |
B _HandleReadRegDone | |
_HandleReadFPS0_FPS31LazyMode: | |
SUBS R1,#+20 // convert abs. RegIndex into rel. one | |
MOVS R0,#+6 | |
MULS R1,R0,R1 | |
LDR R0,=_HandleReadRegUnknown | |
SUB R0,R0,R1 // _HandleReadRegUnknown - 6 * ((RegIndex - 21) + 1) | |
ORR R0,R0,#1 // Thumb bit needs to be set in DestAddr | |
BX R0 | |
// | |
// Table for reading FPS0-FPS31 | |
// | |
VMOV R0,S31 // v = FPSx | |
B _HandleReadRegDone | |
VMOV R0,S30 | |
B _HandleReadRegDone | |
VMOV R0,S29 | |
B _HandleReadRegDone | |
VMOV R0,S28 | |
B _HandleReadRegDone | |
VMOV R0,S27 | |
B _HandleReadRegDone | |
VMOV R0,S26 | |
B _HandleReadRegDone | |
VMOV R0,S25 | |
B _HandleReadRegDone | |
VMOV R0,S24 | |
B _HandleReadRegDone | |
VMOV R0,S23 | |
B _HandleReadRegDone | |
VMOV R0,S22 | |
B _HandleReadRegDone | |
VMOV R0,S21 | |
B _HandleReadRegDone | |
VMOV R0,S20 | |
B _HandleReadRegDone | |
VMOV R0,S19 | |
B _HandleReadRegDone | |
VMOV R0,S18 | |
B _HandleReadRegDone | |
VMOV R0,S17 | |
B _HandleReadRegDone | |
VMOV R0,S16 | |
B _HandleReadRegDone | |
VMOV R0,S15 | |
B _HandleReadRegDone | |
VMOV R0,S14 | |
B _HandleReadRegDone | |
VMOV R0,S13 | |
B _HandleReadRegDone | |
VMOV R0,S12 | |
B _HandleReadRegDone | |
VMOV R0,S11 | |
B _HandleReadRegDone | |
VMOV R0,S10 | |
B _HandleReadRegDone | |
VMOV R0,S9 | |
B _HandleReadRegDone | |
VMOV R0,S8 | |
B _HandleReadRegDone | |
VMOV R0,S7 | |
B _HandleReadRegDone | |
VMOV R0,S6 | |
B _HandleReadRegDone | |
VMOV R0,S5 | |
B _HandleReadRegDone | |
VMOV R0,S4 | |
B _HandleReadRegDone | |
VMOV R0,S3 | |
B _HandleReadRegDone | |
VMOV R0,S2 | |
B _HandleReadRegDone | |
VMOV R0,S1 | |
B _HandleReadRegDone | |
VMOV R0,S0 | |
B _HandleReadRegDone | |
#else | |
B _HandleReadRegUnknown | |
_HandleReadRegDone_Veneer: | |
B _HandleReadRegDone | |
#endif | |
_HandleReadRegUnknown: | |
MOVS R0,#+0 // v = 0 | |
B.N _HandleReadRegDone | |
_HandleReadRegDone: | |
// Send register content to J-Link and wait until J-Link has read the data | |
STR R0,[R3, #+_SYSTEM_DCRDR_OFF] // DCRDR = v; | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Set MON_REQ bit, so J-Link knows monitor is ready to receive commands | |
ORR R0,R0,#0x80000 | |
STR R0,[R3, #+_SYSTEM_DEMCR_OFF] // Indicate data ready | |
B _WaitProbeReadIndicateMonRdy | |
// Data section for register addresses | |
_HandleWriteReg: | |
/* | |
_MON_CMD_WRITE_REG | |
CPU: DEMCR[19] -> 1 => Mon ready | |
J-Link: Data -> DCRDR, DEMCR[19] -> 0 => Data placed by probe | |
CPU: DCRDR -> Read, Process command, DEMCR[19] -> 1 => Data read & mon ready | |
Register indexes | |
0-15: R0-R15 (13 == R13 reserved => is banked ... Has to be read as PSP / MSP. Decision has to be done by J-Link DLL side!) | |
16: XPSR | |
17: MSP | |
18: PSP | |
19: CFBP CONTROL/FAULTMASK/BASEPRI/PRIMASK (packed into 4 bytes of word. CONTROL = CFBP[31:24], FAULTMASK = CFBP[16:23], BASEPRI = CFBP[15:8], PRIMASK = CFBP[7:0] | |
20: FPSCR | |
21-52: FPS0-FPS31 | |
Register usage when entering this "subroutine": | |
R0 Cmd | |
R1 ParamInfo | |
R2 --- | |
R3 = &_SYSTEM_DCRDR (allows also access to DEMCR with offset) | |
R12 --- | |
Table B1-9 EXC_RETURN definition of exception return behavior, with FP extension | |
LR Return to Return SP Frame type | |
--------------------------------------------------------- | |
0xFFFFFFE1 Handler mode. MSP Extended | |
0xFFFFFFE9 Thread mode MSP Extended | |
0xFFFFFFED Thread mode PSP Extended | |
0xFFFFFFF1 Handler mode. MSP Basic | |
0xFFFFFFF9 Thread mode MSP Basic | |
0xFFFFFFFD Thread mode PSP Basic | |
So LR[2] == 1 => Return stack == PSP else MSP | |
R0-R3, R12, PC, xPSR can be written via application stackpointer | |
Other regs can be written directly | |
Read register data from J-Link into R0 | |
*/ | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Monitor is ready to receive register data | |
ORR R0,R0,#0x80000 | |
STR R0,[R3, #+_SYSTEM_DEMCR_OFF] | |
_HandleWRegWaitUntilDataRecv: | |
LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] | |
LSLS R0,R0,#+12 | |
BMI.N _HandleWRegWaitUntilDataRecv // DEMCR[19] == 0 => J-Link has placed new data for us | |
LDR R0,[R3, #+_SYSTEM_DCRDR_OFF] // Get register data | |
// | |
// Determine application SP | |
// | |
LSRS R2,LR,#+3 // Shift LR[2] into carry => Carry clear means that CPU was running on MSP | |
ITE CS | |
MRSCS R2,PSP | |
MRSCC R2,MSP | |
CMP R1,#+4 // if (RegIndex < 4) { (R0-R3) | |
BCS _HandleWriteRegR4 | |
STR R0,[R2, R1, LSL #+2] // v = [SP + Rx * 4] (R0-R3) | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR4: | |
CMP R1,#+5 // if (RegIndex < 5) { (R4) | |
BCS _HandleWriteRegR5 | |
MOV R4,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR5: | |
CMP R1,#+6 // if (RegIndex < 6) { (R5) | |
BCS _HandleWriteRegR6 | |
MOV R5,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR6: | |
CMP R1,#+7 // if (RegIndex < 7) { (R6) | |
BCS _HandleWriteRegR7 | |
MOV R6,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR7: | |
CMP R1,#+8 // if (RegIndex < 8) { (R7) | |
BCS _HandleWriteRegR8 | |
MOV R7,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR8: | |
CMP R1,#+9 // if (RegIndex < 9) { (R8) | |
BCS _HandleWriteRegR9 | |
MOV R8,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR9: | |
CMP R1,#+10 // if (RegIndex < 10) { (R9) | |
BCS _HandleWriteRegR10 | |
MOV R9,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR10: | |
CMP R1,#+11 // if (RegIndex < 11) { (R10) | |
BCS _HandleWriteRegR11 | |
MOV R10,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR11: | |
CMP R1,#+12 // if (RegIndex < 12) { (R11) | |
BCS _HandleWriteRegR12 | |
MOV R11,R0 | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR12: | |
CMP R1,#+14 // if (RegIndex < 14) { (R12) | |
BCS _HandleWriteRegR14 | |
STR R0,[R2, #+_APP_SP_OFF_R12] | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR14: | |
CMP R1,#+15 // if (RegIndex < 15) { (R14 / LR) | |
BCS _HandleWriteRegR15 | |
STR R0,[R2, #+_APP_SP_OFF_R14_LR] | |
B.N _HandleWriteRegDone | |
_HandleWriteRegR15: | |
CMP R1,#+16 // if (RegIndex < 16) { (R15 / PC) | |
BCS _HandleWriteRegXPSR | |
STR R0,[R2, #+_APP_SP_OFF_PC] | |
B.N _HandleWriteRegDone | |
_HandleWriteRegXPSR: | |
CMP R1,#+17 // if (RegIndex < 17) { (xPSR) | |
BCS _HandleWriteRegMSP | |
STR R0,[R2, #+_APP_SP_OFF_XPSR] | |
B.N _HandleWriteRegDone | |
_HandleWriteRegMSP: | |
// | |
// For now, SP cannot be modified because it is needed to jump back from monitor mode | |
// | |
CMP R1,#+18 // if (RegIndex < 18) { (MSP) | |
BCS _HandleWriteRegPSP | |
B.N _HandleWriteRegDone | |
_HandleWriteRegPSP: // RegIndex == 18 | |
CMP R1,#+19 // if (RegIndex < 19) { | |
BCS _HandleWriteRegCFBP | |
B.N _HandleWriteRegDone | |
_HandleWriteRegCFBP: | |
/* | |
CFBP is a register that can only be read via debug probe and is a merger of the following regs: | |
CONTROL/FAULTMASK/BASEPRI/PRIMASK (packed into 4 bytes of word. CONTROL = CFBP[31:24], FAULTMASK = CFBP[16:23], BASEPRI = CFBP[15:8], PRIMASK = CFBP[7:0] | |
To keep J-Link side the same for monitor and halt mode, we also return CFBP in monitor mode | |
*/ | |
CMP R1,#+20 // if (RegIndex < 20) { (CFBP) | |
BCS _HandleWriteRegFPU | |
LSLS R1,R0,#+24 | |
LSRS R1,R1,#+24 // Extract CFBP[7:0] => PRIMASK | |
MSR PRIMASK,R1 | |
LSLS R1,R0,#+16 | |
LSRS R1,R1,#+24 // Extract CFBP[15:8] => BASEPRI | |
MSR BASEPRI,R1 | |
LSLS R1,R0,#+8 // Extract CFBP[23:16] => FAULTMASK | |
LSRS R1,R1,#+24 | |
MSR FAULTMASK,R1 | |
LSRS R1,R0,#+24 // Extract CFBP[31:24] => CONTROL | |
LSRS R0,R1,#2 // Current CONTROL[1] -> Carry | |
ITE CS // Update saved CONTROL.SPSEL (CONTROL[1]). CONTROL.SPSEL is saved to LR[2] on exception entry => ARM DDI0403D, B1.5.6 Exception entry behavior | |
ORRCS LR,LR,#+4 | |
BICCC LR,LR,#+4 | |
BIC R1,R1,#+2 // CONTROL.SPSEL (CONTROL[1]) == 0 inside monitor. Otherwise behavior is UNPREDICTABLE | |
LSRS R0,R1,#+3 // New CONTROL.FPCA (CONTROL[2]) -> Carry | |
ITE CS // CONTROL[2] == FPCA => NOT(FPCA) saved to LR[4]. LR[4] == 0 => Extended stack frame, so FPU regs possibly on stack | |
BICCS LR,LR,#+0x10 // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame | |
ORRCC LR,LR,#+0x10 | |
MRS R0,CONTROL | |
LSRS R0,R0,#+3 // CONTROL[2] -> Carry | |
ITE CS // Preserve original value of current CONTROL[2] | |
ORRCS R1,R1,#+0x04 | |
BICCC R1,R1,#+0x04 | |
MSR CONTROL,R1 | |
ISB // Necessary after writing to CONTROL, see ARM DDI0403D, B1.4.4 The special-purpose CONTROL register | |
B.N _HandleWriteRegDone | |
_HandleWriteRegFPU: | |
#if _HAS_FPU_REGS | |
CMP R1,#+53 // if (RegIndex < 53) { (20 (FPSCR), 21-52 FPS0-FPS31) | |
BCS _HandleWriteRegDone_Veneer | |
/* | |
Read Coprocessor Access Control Register (CPACR) to check if CP10 and CP11 are enabled | |
If not, access to floating point is not possible | |
CPACR[21:20] == CP10 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved | |
CPACR[23:22] == CP11 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved | |
*/ | |
MOV R12,R0 // Save register data | |
LDR R0,_AddrCPACR | |
LDR R0,[R0] | |
LSLS R0,R0,#+8 | |
LSRS R0,R0,#+28 | |
CMP R0,#+0xF | |
BEQ _HandleWriteRegFPU_Allowed | |
CMP R0,#+0x5 | |
BNE _HandleWriteRegDone_Veneer | |
_HandleWriteRegFPU_Allowed: | |
CMP R1,#+21 // if (RegIndex < 21) (20 == FPSCR) | |
BCS _HandleWriteRegFPS0_FPS31 | |
LSRS R0,LR,#+5 // CONTROL[2] == FPCA => NOT(FPCA) saved to LR[4]. LR[4] == 0 => Extended stack frame, so FPU regs possibly on stack | |
BCS _HandleWriteFPSCRLazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame | |
LDR R0,=_SYSTEM_FPCCR | |
LDR R0,[R0] | |
LSLS R0,R0,#+2 // FPCCR[30] -> Carry == 1 indicates if lazy mode is active, so space on stack is reserved but FPU registers are not saved on stack | |
BCS _HandleWriteFPSCRLazyMode | |
STR R12,[R2, #+_APP_SP_OFF_FPSCR] | |
B _HandleWriteRegDone | |
_HandleWriteFPSCRLazyMode: | |
VMSR FPSCR,R12 | |
B _HandleWriteRegDone | |
_HandleWriteRegFPS0_FPS31: // RegIndex == 21-52 | |
LDR R0,=_SYSTEM_FPCCR | |
LDR R0,[R0] | |
LSLS R0,R0,#+2 // FPCCR[30] -> Carry == 1 indicates if lazy mode is active, so space on stack is reserved but FPU registers are not saved on stack | |
BCS _HandleWriteFPS0_FPS31LazyMode | |
LSRS R0,LR,#+5 // CONTROL[2] == FPCA => NOT(FPCA) saved to LR[4]. LR[4] == 0 => Extended stack frame, so FPU regs possibly on stack | |
BCS _HandleWriteFPS0_FPS31LazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame | |
SUBS R1,#+21 // Convert absolute reg index into rel. one | |
LSLS R1,R1,#+2 // RegIndex to position on stack | |
ADDS R1,#+_APP_SP_OFF_S0 | |
STR R12,[R2, R1] | |
_HandleWriteRegDone_Veneer: | |
B _HandleWriteRegDone | |
_HandleWriteFPS0_FPS31LazyMode: | |
SUBS R1,#+20 // Convert abs. RegIndex into rel. one | |
MOVS R0,#+6 | |
MULS R1,R0,R1 | |
LDR R0,=_HandleReadRegUnknown | |
SUB R0,R0,R1 // _HandleReadRegUnknown - 6 * ((RegIndex - 21) + 1) | |
ORR R0,R0,#1 // Thumb bit needs to be set in DestAddr | |
BX R0 | |
// | |
// Table for reading FPS0-FPS31 | |
// | |
VMOV S31,R12 // v = FPSx | |
B _HandleWriteRegDone | |
VMOV S30,R12 | |
B _HandleWriteRegDone | |
VMOV S29,R12 | |
B _HandleWriteRegDone | |
VMOV S28,R12 | |
B _HandleWriteRegDone | |
VMOV S27,R12 | |
B _HandleWriteRegDone | |
VMOV S26,R12 | |
B _HandleWriteRegDone | |
VMOV S25,R12 | |
B _HandleWriteRegDone | |
VMOV S24,R12 | |
B _HandleWriteRegDone | |
VMOV S23,R12 | |
B _HandleWriteRegDone | |
VMOV S22,R12 | |
B _HandleWriteRegDone | |
VMOV S21,R12 | |
B _HandleWriteRegDone | |
VMOV S20,R12 | |
B _HandleWriteRegDone | |
VMOV S19,R12 | |
B _HandleWriteRegDone | |
VMOV S18,R12 | |
B _HandleWriteRegDone | |
VMOV S17,R12 | |
B _HandleWriteRegDone | |
VMOV S16,R12 | |
B _HandleWriteRegDone | |
VMOV S15,R12 | |
B _HandleWriteRegDone | |
VMOV S14,R12 | |
B _HandleWriteRegDone | |
VMOV S13,R12 | |
B _HandleWriteRegDone | |
VMOV S12,R12 | |
B _HandleWriteRegDone | |
VMOV S11,R12 | |
B _HandleWriteRegDone | |
VMOV S10,R12 | |
B _HandleWriteRegDone | |
VMOV S9,R12 | |
B _HandleWriteRegDone | |
VMOV S8,R12 | |
B _HandleWriteRegDone | |
VMOV S7,R12 | |
B _HandleWriteRegDone | |
VMOV S6,R12 | |
B _HandleWriteRegDone | |
VMOV S5,R12 | |
B _HandleWriteRegDone | |
VMOV S4,R12 | |
B _HandleWriteRegDone | |
VMOV S3,R12 | |
B _HandleWriteRegDone | |
VMOV S2,R12 | |
B _HandleWriteRegDone | |
VMOV S1,R12 | |
B _HandleWriteRegDone | |
VMOV S0,R12 | |
B _HandleWriteRegDone | |
#else | |
B _HandleWriteRegUnknown | |
#endif | |
_HandleWriteRegUnknown: | |
B.N _HandleWriteRegDone | |
_HandleWriteRegDone: | |
B _IndicateMonReady // Indicate that monitor has read data, processed command and is ready for a new one | |
// .end | |
END | |
/****** End Of File *************************************************/ |