From 783466f1165aeddd41ab1b1b76ef70aa2908c3b1 Mon Sep 17 00:00:00 2001 From: Ben Meadors Date: Sat, 5 Oct 2024 05:24:59 -0500 Subject: [PATCH] Revert "Remove unused Jlink monitoring files (#4953)" (#4959) This reverts commit 55049ed547103787d700e0df5331f88755b89d8d. --- src/platform/nrf52/JLINK_MONITOR.c | 124 +++ src/platform/nrf52/JLINK_MONITOR.h | 27 + src/platform/nrf52/JLINK_MONITOR_ISR_SES.S | 888 +++++++++++++++++++++ 3 files changed, 1039 insertions(+) create mode 100644 src/platform/nrf52/JLINK_MONITOR.c create mode 100644 src/platform/nrf52/JLINK_MONITOR.h create mode 100644 src/platform/nrf52/JLINK_MONITOR_ISR_SES.S diff --git a/src/platform/nrf52/JLINK_MONITOR.c b/src/platform/nrf52/JLINK_MONITOR.c new file mode 100644 index 000000000..160264905 --- /dev/null +++ b/src/platform/nrf52/JLINK_MONITOR.c @@ -0,0 +1,124 @@ +/********************************************************************* +* 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.c +Purpose : Implementation of debug monitor for J-Link monitor mode debug on Cortex-M devices. +-------- END-OF-HEADER --------------------------------------------- +*/ + +#include "JLINK_MONITOR.h" + +/********************************************************************* + * + * Configuration + * + ********************************************************************** + */ + +/********************************************************************* + * + * Defines + * + ********************************************************************** + */ + +/********************************************************************* + * + * Types + * + ********************************************************************** + */ + +/********************************************************************* + * + * Static data + * + ********************************************************************** + */ + +volatile int MAIN_MonCnt; // Incremented in JLINK_MONITOR_OnPoll() while CPU is in debug mode + +/********************************************************************* + * + * Local functions + * + ********************************************************************** + */ + +/********************************************************************* + * + * Global functions + * + ********************************************************************** + */ + +/********************************************************************* + * + * JLINK_MONITOR_OnExit() + * + * Function description + * Called from DebugMon_Handler(), once per debug exit. + * May perform some target specific operations to be done on debug mode exit. + * + * Notes + * (1) Must not keep the CPU busy for more than 100 ms + */ +void JLINK_MONITOR_OnExit(void) +{ + // + // Add custom code here + // + // BSP_ClrLED(0); +} + +/********************************************************************* + * + * JLINK_MONITOR_OnEnter() + * + * Function description + * Called from DebugMon_Handler(), once per debug entry. + * May perform some target specific operations to be done on debug mode entry + * + * Notes + * (1) Must not keep the CPU busy for more than 100 ms + */ +void JLINK_MONITOR_OnEnter(void) +{ + // + // Add custom code here + // + // BSP_SetLED(0); + // BSP_ClrLED(1); +} + +/********************************************************************* + * + * JLINK_MONITOR_OnPoll() + * + * Function description + * Called periodically from DebugMon_Handler(), to perform some actions that need to be performed periodically during debug + * mode. + * + * Notes + * (1) Must not keep the CPU busy for more than 100 ms + */ +void JLINK_MONITOR_OnPoll(void) +{ + // + // Add custom code here + // + MAIN_MonCnt++; + // BSP_ToggleLED(0); + // _Delay(500000); +} + +/****** End Of File *************************************************/ diff --git a/src/platform/nrf52/JLINK_MONITOR.h b/src/platform/nrf52/JLINK_MONITOR.h new file mode 100644 index 000000000..87cf332fe --- /dev/null +++ b/src/platform/nrf52/JLINK_MONITOR.h @@ -0,0 +1,27 @@ +/********************************************************************* +* 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.h +Purpose : Header file of debug monitor for J-Link monitor mode debug on Cortex-M devices. +-------- END-OF-HEADER --------------------------------------------- +*/ + +#ifndef JLINK_MONITOR_H +#define JLINK_MONITOR_H + +void JLINK_MONITOR_OnExit(void); +void JLINK_MONITOR_OnEnter(void); +void JLINK_MONITOR_OnPoll(void); + +#endif + +/****** End Of File *************************************************/ diff --git a/src/platform/nrf52/JLINK_MONITOR_ISR_SES.S b/src/platform/nrf52/JLINK_MONITOR_ISR_SES.S new file mode 100644 index 000000000..cda4b1a50 --- /dev/null +++ b/src/platform/nrf52/JLINK_MONITOR_ISR_SES.S @@ -0,0 +1,888 @@ +/********************************************************************* +* 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 + .syntax unified + + .extern JLINK_MONITOR_OnEnter + .extern JLINK_MONITOR_OnExit + .extern JLINK_MONITOR_OnPoll + + .global 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" + + // + // JLINKMONHANDLER + // + .byte 0x4A + .byte 0x4C + .byte 0x49 + .byte 0x4E + .byte 0x4B + .byte 0x4D + .byte 0x4F + .byte 0x4E + .byte 0x48 + .byte 0x41 + .byte 0x4E + .byte 0x44 + .byte 0x4C + .byte 0x45 + .byte 0x52 + .byte 0x00 // Align to 8-bytes + +/********************************************************************* +* +* 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 interrupt (exception no. 12) +*/ + .thumb_func + +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 + .align 2 +_AddrDCRDR: + .long 0xE000EDF8 +_AddrCPACR: + .long 0xE000ED88 + + .section .text, "ax" + .thumb_func + +;/********************************************************************* +;* +;* _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 Of File *************************************************/