1/********************************************************************* 2* (c) SEGGER Microcontroller GmbH * 3* The Embedded Experts * 4* www.segger.com * 5********************************************************************** 6* * 7* (c) 1995 - 2020 SEGGER Microcontroller GmbH * 8* * 9* www.segger.com Support: support@segger.com * 10* * 11********************************************************************** 12* * 13* All rights reserved. * 14* * 15* SEGGER strongly recommends to not make any changes * 16* to or modify the source code of this software in order to stay * 17* compatible with the monitor mode protocol and J-Link. * 18* * 19* Redistribution and use in source and binary forms, with or * 20* without modification, are permitted provided that the following * 21* conditions are met: * 22* * 23* - Redistributions of source code must retain the above copyright * 24* notice, this list of conditions and the following disclaimer. * 25* * 26* - Redistributions in binary form must reproduce the above * 27* copyright notice, this list of conditions and the following * 28* disclaimer in the documentation and/or other materials provided * 29* with the distribution. * 30* * 31* - Neither the name of SEGGER Microcontroller GmbH * 32* nor the names of its contributors may be used to endorse or * 33* promote products derived from this software without specific * 34* prior written permission. * 35* * 36* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND * 37* CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, * 38* INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF * 39* MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE * 40* DISCLAIMED. IN NO EVENT SHALL SEGGER Microcontroller BE LIABLE FOR * 41* ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR * 42* CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT * 43* OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; * 44* OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF * 45* LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT * 46* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE * 47* USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH * 48* DAMAGE. * 49* * 50********************************************************************** 51---------------------------------------------------------------------- 52File : JLINK_MONITOR_ISR_SES.s 53Purpose : Implementation of debug monitor for J-Link monitor mode 54 debug on Cortex-M devices, supporting SES compiler. 55-------- END-OF-HEADER --------------------------------------------- 56*/ 57 58 .name JLINK_MONITOR_ISR 59 .syntax unified 60 61 .extern JLINK_MONITOR_OnEnter 62 .extern JLINK_MONITOR_OnExit 63 .extern JLINK_MONITOR_OnPoll 64 65 .global z_arm_debug_monitor 66 67/********************************************************************* 68* 69* Defines, configurable 70* 71********************************************************************** 72*/ 73 74#define _MON_VERSION 100 // V x.yy 75 76/********************************************************************* 77* 78* Defines, fixed 79* 80********************************************************************** 81*/ 82 83#define _APP_SP_OFF_R0 0x00 84#define _APP_SP_OFF_R1 0x04 85#define _APP_SP_OFF_R2 0x08 86#define _APP_SP_OFF_R3 0x0C 87#define _APP_SP_OFF_R12 0x10 88#define _APP_SP_OFF_R14_LR 0x14 89#define _APP_SP_OFF_PC 0x18 90#define _APP_SP_OFF_XPSR 0x1C 91#define _APP_SP_OFF_S0 0x20 92#define _APP_SP_OFF_S1 0x24 93#define _APP_SP_OFF_S2 0x28 94#define _APP_SP_OFF_S3 0x2C 95#define _APP_SP_OFF_S4 0x30 96#define _APP_SP_OFF_S5 0x34 97#define _APP_SP_OFF_S6 0x38 98#define _APP_SP_OFF_S7 0x3C 99#define _APP_SP_OFF_S8 0x40 100#define _APP_SP_OFF_S9 0x44 101#define _APP_SP_OFF_S10 0x48 102#define _APP_SP_OFF_S11 0x4C 103#define _APP_SP_OFF_S12 0x50 104#define _APP_SP_OFF_S13 0x54 105#define _APP_SP_OFF_S14 0x58 106#define _APP_SP_OFF_S15 0x5C 107#define _APP_SP_OFF_FPSCR 0x60 108 109#define _NUM_BYTES_BASIC_STACKFRAME 32 110#define _NUM_BYTES_EXTENDED_STACKFRAME 104 // Values for stackframes are explained at location where they are used 111 112#define _SYSTEM_DCRDR_OFF 0x00 113#define _SYSTEM_DEMCR_OFF 0x04 114 115#define _SYSTEM_DHCSR 0xE000EDF0 // Debug Halting Control and Status Register (DHCSR) 116#define _SYSTEM_DCRSR 0xE000EDF4 // Debug Core Register Selector Register (DCRSR) 117#define _SYSTEM_DCRDR 0xE000EDF8 // Debug Core Register Data Register (DCRDR) 118#define _SYSTEM_DEMCR 0xE000EDFC // Debug Exception and Monitor Control Register (DEMCR) 119 120#define _SYSTEM_FPCCR 0xE000EF34 // Floating-Point Context Control Register (FPCCR) 121#define _SYSTEM_FPCAR 0xE000EF38 // Floating-Point Context Address Register (FPCAR) 122#define _SYSTEM_FPDSCR 0xE000EF3C // Floating-Point Default Status Control Register (FPDSCR) 123#define _SYSTEM_MVFR0 0xE000EF40 // Media and FP Feature Register 0 (MVFR0) 124#define _SYSTEM_MVFR1 0xE000EF44 // Media and FP Feature Register 1 (MVFR1) 125 126/* 127* Defines for determining if the current debug config supports FPU registers 128* For some compilers like IAR EWARM when disabling the FPU in the compiler settings an error is thrown when 129*/ 130#ifdef __FPU_PRESENT 131 #if __FPU_PRESENT 132 #define _HAS_FPU_REGS 1 133 #else 134 #define _HAS_FPU_REGS 0 135 #endif 136#else 137 #define _HAS_FPU_REGS 0 138#endif 139 140/********************************************************************* 141* 142* Signature of monitor 143* 144* Function description 145* Needed for targets where also a boot ROM is present that possibly specifies a vector table with a valid debug monitor exception entry 146*/ 147 .section .text, "ax" 148 149 // 150 // JLINKMONHANDLER 151 // 152 .byte 0x4A 153 .byte 0x4C 154 .byte 0x49 155 .byte 0x4E 156 .byte 0x4B 157 .byte 0x4D 158 .byte 0x4F 159 .byte 0x4E 160 .byte 0x48 161 .byte 0x41 162 .byte 0x4E 163 .byte 0x44 164 .byte 0x4C 165 .byte 0x45 166 .byte 0x52 167 .byte 0x00 // Align to 8-bytes 168 169/********************************************************************* 170* 171* z_arm_debug_monitor() 172* 173* Function description 174* Debug monitor handler. CPU enters this handler in case a "halt" request is made from the debugger. 175* This handler is also responsible for handling commands that are sent by the debugger. 176* 177* Notes 178* This is actually the ISR for the debug inerrupt (exception no. 12) 179*/ 180 .thumb_func 181 182z_arm_debug_monitor: 183 /* 184 General procedure: 185 DCRDR is used as communication register 186 DEMCR[19] is used as ready flag 187 For the command J-Link sends to the monitor: DCRDR[7:0] == Cmd, DCRDR[31:8] == ParamData 188 189 1) Monitor sets DEMCR[19] whenever it is ready to receive new commands/data 190 DEMCR[19] is initially set on debug monitor entry 191 2) J-Link will clear DEMCR[19] it has placed command/data in DCRDR for monitor 192 3) Monitor will wait for DEMCR[19] to be cleared 193 4) Monitor will process command (May cause additional data transfers etc., depends on command). Monitor will set DEMCR[19] whenever it has placed data for J-Link in the DCRDR 194 5) No restart-CPU command? => Back to 2), Otherwise => 6) 195 6) Monitor will clear DEMCR[19] 19 to indicate that it is no longer ready 196 */ 197 PUSH {LR} 198 BL JLINK_MONITOR_OnEnter 199 POP {LR} 200 LDR.N R3,_AddrDCRDR // 0xe000edf8 == _SYSTEM_DCRDR 201 B.N _IndicateMonReady 202_WaitProbeReadIndicateMonRdy: // while(_SYSTEM_DEMCR & (1uL << 19)); => Wait until J-Link has read item 203 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR 204 LSLS R0,R0,#+12 205 BMI.N _WaitProbeReadIndicateMonRdy 206_IndicateMonReady: 207 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Set MON_REQ bit, so J-Link knows monitor is ready to receive commands 208 ORR R0,R0,#0x80000 209 STR R0,[R3, #+_SYSTEM_DEMCR_OFF] 210 /* 211 During command loop: 212 R0 = Tmp 213 R1 = Tmp 214 R2 = Tmp 215 R3 = &_SYSTEM_DCRDR (allows also access to DEMCR with offset) 216 R12 = Tmp 217 218 Outside command loop R0-R3 and R12 may be overwritten by MONITOR_OnPoll() 219 */ 220_WaitForJLinkCmd: // do { 221 PUSH {LR} 222 BL JLINK_MONITOR_OnPoll 223 POP {LR} 224 LDR.N R3,_AddrDCRDR // 0xe000edf8 == _SYSTEM_DCRDR 225 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] 226 LSRS R0,R0,#+20 // DEMCR[19] -> Carry Clear? => J-Link has placed command for us 227 BCS _WaitForJLinkCmd 228 /* 229 Perform command 230 Command is placed by J-Link in DCRDR[7:0] and additional parameter data is stored in DCRDR[31:8] 231 J-Link clears DEMCR[19] to indicate that it placed a command/data or read data 232 Monitor sets DEMCR[19] to indicate that it placed data or read data / is ready for a new command 233 Setting DEMCR[19] indicates "monitor ready for new command / data" and also indicates: "data has been placed in DCRDR by monitor, for J-Link" 234 Therefore it is responsibility of the commands to respond to the commands accordingly 235 236 Commands for debug monitor 237 Commands must not exceed 0xFF (255) as we only defined 8-bits for command-part. Higher 24-bits are parameter info for current command 238 239 Protocol for different commands: 240 J-Link: Cmd -> DCRDR, DEMCR[19] -> 0 => Cmd placed by probe 241 */ 242 LDR R0,[R3, #+_SYSTEM_DCRDR_OFF] // ParamInfo = _SYSTEM_DCRDR 243 LSRS R1,R0,#+8 // ParamInfo >>= 8 244 LSLS R0,R0,#+24 245 LSRS R0,R0,#+24 // Cmd = ParamInfo & 0xFF 246 // 247 // switch (Cmd) 248 // 249 CMP R0,#+0 250 BEQ.N _HandleGetMonVersion // case _MON_CMD_GET_MONITOR_VERSION 251 CMP R0,#+2 252 BEQ.N _HandleReadReg // case _MON_CMD_READ_REG 253 BCC.N _HandleRestartCPU // case _MON_CMD_RESTART_CPU 254 CMP R0,#+3 255 BEQ.N _HandleWriteReg_Veneer // case _MON_CMD_WRITE_REG 256 B.N _IndicateMonReady // default : while (1); 257 /* 258 Return 259 _MON_CMD_RESTART_CPU 260 CPU: DEMCR[19] -> 0 => Monitor no longer ready 261 */ 262_HandleRestartCPU: 263 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR &= ~(1uL << 19); => Clear MON_REQ to indicate that monitor is no longer active 264 BIC R0,R0,#0x80000 265 STR R0,[R3, #+_SYSTEM_DEMCR_OFF] 266 PUSH {LR} 267 BL JLINK_MONITOR_OnExit 268 POP {PC} 269 // 270 // Place data section here to not get in trouble with load-offsets 271 // 272 .section .text, "ax", %progbits 273 .align 2 274_AddrDCRDR: 275 .long 0xE000EDF8 276_AddrCPACR: 277 .long 0xE000ED88 278 279 .section .text, "ax" 280 .thumb_func 281 282;/********************************************************************* 283;* 284;* _HandleGetMonVersion 285;* 286;*/ 287_HandleGetMonVersion: 288 /* 289 _MON_CMD_GET_MONITOR_VERSION 290 CPU: Data -> DCRDR, DEMCR[19] -> 1 => Data ready 291 J-Link: DCRDR -> Read, DEMCR[19] -> 0 => Data read 292 CPU: DEMCR[19] -> 1 => Mon ready 293 */ 294 MOVS R0,#+_MON_VERSION 295 STR R0,[R3, #+_SYSTEM_DCRDR_OFF] // _SYSTEM_DCRDR = x 296 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Set MON_REQ bit, so J-Link knows monitor is ready to receive commands 297 ORR R0,R0,#0x80000 298 STR R0,[R3, #+_SYSTEM_DEMCR_OFF] // Indicate data ready 299 B _WaitProbeReadIndicateMonRdy 300 301/********************************************************************* 302* 303* _HandleReadReg 304* 305*/ 306_HandleWriteReg_Veneer: 307 B.N _HandleWriteReg 308_HandleReadReg: 309 /* 310 _MON_CMD_READ_REG 311 CPU: Data -> DCRDR, DEMCR[19] -> 1 => Data ready 312 J-Link: DCRDR -> Read, DEMCR[19] -> 0 => Data read 313 CPU: DEMCR[19] -> 1 => Mon ready 314 315 316 Register indexes 317 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!) 318 16: XPSR 319 17: MSP 320 18: PSP 321 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] 322 20: FPSCR 323 21-52: FPS0-FPS31 324 325 326 Register usage when entering this "subroutine": 327 R0 Cmd 328 R1 ParamInfo 329 R2 --- 330 R3 = &_SYSTEM_DCRDR (allows also access to DEMCR with offset) 331 R12 --- 332 333 Table B1-9 EXC_RETURN definition of exception return behavior, with FP extension 334 LR Return to Return SP Frame type 335 --------------------------------------------------------- 336 0xFFFFFFE1 Handler mode. MSP Extended 337 0xFFFFFFE9 Thread mode MSP Extended 338 0xFFFFFFED Thread mode PSP Extended 339 0xFFFFFFF1 Handler mode. MSP Basic 340 0xFFFFFFF9 Thread mode MSP Basic 341 0xFFFFFFFD Thread mode PSP Basic 342 343 So LR[2] == 1 => Return stack == PSP else MSP 344 345 R0-R3, R12, PC, xPSR can be read from application stackpointer 346 Other regs can be read directly 347 */ 348 LSRS R2,LR,#+3 // Shift LR[2] into carry => Carry clear means that CPU was running on MSP 349 ITE CS 350 MRSCS R2,PSP 351 MRSCC R2,MSP 352 CMP R1,#+4 // if (RegIndex < 4) { (R0-R3) 353 BCS _HandleReadRegR4 354 LDR R0,[R2, R1, LSL #+2] // v = [SP + Rx * 4] (R0-R3) 355 B.N _HandleReadRegDone 356_HandleReadRegR4: 357 CMP R1,#+5 // if (RegIndex < 5) { (R4) 358 BCS _HandleReadRegR5 359 MOV R0,R4 360 B.N _HandleReadRegDone 361_HandleReadRegR5: 362 CMP R1,#+6 // if (RegIndex < 6) { (R5) 363 BCS _HandleReadRegR6 364 MOV R0,R5 365 B.N _HandleReadRegDone 366_HandleReadRegR6: 367 CMP R1,#+7 // if (RegIndex < 7) { (R6) 368 BCS _HandleReadRegR7 369 MOV R0,R6 370 B.N _HandleReadRegDone 371_HandleReadRegR7: 372 CMP R1,#+8 // if (RegIndex < 8) { (R7) 373 BCS _HandleReadRegR8 374 MOV R0,R7 375 B.N _HandleReadRegDone 376_HandleReadRegR8: 377 CMP R1,#+9 // if (RegIndex < 9) { (R8) 378 BCS _HandleReadRegR9 379 MOV R0,R8 380 B.N _HandleReadRegDone 381_HandleReadRegR9: 382 CMP R1,#+10 // if (RegIndex < 10) { (R9) 383 BCS _HandleReadRegR10 384 MOV R0,R9 385 B.N _HandleReadRegDone 386_HandleReadRegR10: 387 CMP R1,#+11 // if (RegIndex < 11) { (R10) 388 BCS _HandleReadRegR11 389 MOV R0,R10 390 B.N _HandleReadRegDone 391_HandleReadRegR11: 392 CMP R1,#+12 // if (RegIndex < 12) { (R11) 393 BCS _HandleReadRegR12 394 MOV R0,R11 395 B.N _HandleReadRegDone 396_HandleReadRegR12: 397 CMP R1,#+14 // if (RegIndex < 14) { (R12) 398 BCS _HandleReadRegR14 399 LDR R0,[R2, #+_APP_SP_OFF_R12] 400 B.N _HandleReadRegDone 401_HandleReadRegR14: 402 CMP R1,#+15 // if (RegIndex < 15) { (R14 / LR) 403 BCS _HandleReadRegR15 404 LDR R0,[R2, #+_APP_SP_OFF_R14_LR] 405 B.N _HandleReadRegDone 406_HandleReadRegR15: 407 CMP R1,#+16 // if (RegIndex < 16) { (R15 / PC) 408 BCS _HandleReadRegXPSR 409 LDR R0,[R2, #+_APP_SP_OFF_PC] 410 B.N _HandleReadRegDone 411_HandleReadRegXPSR: 412 CMP R1,#+17 // if (RegIndex < 17) { (xPSR) 413 BCS _HandleReadRegMSP 414 LDR R0,[R2, #+_APP_SP_OFF_XPSR] 415 B.N _HandleReadRegDone 416_HandleReadRegMSP: 417 /* 418 Stackpointer is tricky because we need to get some info about the SP used in the user app, first 419 420 Handle reading R0-R3 which can be read right from application stackpointer 421 422 Table B1-9 EXC_RETURN definition of exception return behavior, with FP extension 423 LR Return to Return SP Frame type 424 --------------------------------------------------------- 425 0xFFFFFFE1 Handler mode. MSP Extended 426 0xFFFFFFE9 Thread mode MSP Extended 427 0xFFFFFFED Thread mode PSP Extended 428 0xFFFFFFF1 Handler mode. MSP Basic 429 0xFFFFFFF9 Thread mode MSP Basic 430 0xFFFFFFFD Thread mode PSP Basic 431 432 So LR[2] == 1 => Return stack == PSP else MSP 433 Per architecture definition: Inside monitor (exception) SP = MSP 434 435 Stack pointer handling is complicated because it is different what is pushed on the stack before entering the monitor ISR... 436 Cortex-M: 8 regs 437 Cortex-M + forced-stack-alignment: 8 regs + 1 dummy-word if stack was not 8-byte aligned 438 Cortex-M + FPU: 8 regs + 17 FPU regs + 1 dummy-word + 1-dummy word if stack was not 8-byte aligned 439 Cortex-M + FPU + lazy mode: 8 regs + 17 dummy-words + 1 dummy-word + 1-dummy word if stack was not 8-byte aligned 440 */ 441 CMP R1,#+18 // if (RegIndex < 18) { (MSP) 442 BCS _HandleReadRegPSP 443 MRS R0,MSP 444 LSRS R1,LR,#+3 // LR[2] -> Carry == 0 => CPU was running on MSP => Needs correction 445 BCS _HandleReadRegDone_Veneer // CPU was running on PSP? => No correction necessary 446_HandleSPCorrection: 447 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 448 ITE CS 449 ADDCS R0,R0,#+_NUM_BYTES_BASIC_STACKFRAME 450 ADDCC R0,R0,#+_NUM_BYTES_EXTENDED_STACKFRAME 451 LDR R1,[R2, #+_APP_SP_OFF_XPSR] // Get xPSR from application stack (R2 has been set to app stack on beginning of _HandleReadReg) 452 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 453 IT CS 454 ADDCS R0,R0,#+4 455 B _HandleReadRegDone 456_HandleReadRegPSP: // RegIndex == 18 457 CMP R1,#+19 // if (RegIndex < 19) { 458 BCS _HandleReadRegCFBP 459 MRS R0,PSP // PSP is not touched by monitor 460 LSRS R1,LR,#+3 // LR[2] -> Carry == 1 => CPU was running on PSP => Needs correction 461 BCC _HandleReadRegDone_Veneer // CPU was running on MSP? => No correction of PSP necessary 462 B _HandleSPCorrection 463_HandleReadRegCFBP: 464 /* 465 CFBP is a register that can only be read via debug probe and is a merger of the following regs: 466 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] 467 To keep J-Link side the same for monitor and halt mode, we also return CFBP in monitor mode 468 */ 469 CMP R1,#+20 // if (RegIndex < 20) { (CFBP) 470 BCS _HandleReadRegFPU 471 MOVS R0,#+0 472 MRS R2,PRIMASK 473 ORRS R0,R2 // Merge PRIMASK into CFBP[7:0] 474 MRS R2,BASEPRI 475 LSLS R2,R2,#+8 // Merge BASEPRI into CFBP[15:8] 476 ORRS R0,R2 477 MRS R2,FAULTMASK 478 LSLS R2,R2,#+16 // Merge FAULTMASK into CFBP[23:16] 479 ORRS R0,R2 480 MRS R2,CONTROL 481 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 482 IT CS // As J-Link sees value of CONTROL at application time, we need reconstruct original value of CONTROL 483 ORRCS R2,R2,#+2 // CONTROL.SPSEL (CONTROL[1]) == 0 inside monitor 484 LSRS R1,LR,#+5 // LR[4] == NOT(CONTROL.FPCA) -> Carry 485 ITE CS // Merge original value of FPCA (CONTROL[2]) into read data 486 BICCS R2,R2,#+0x04 // Remember LR contains NOT(CONTROL) 487 ORRCC R2,R2,#+0x04 488 LSLS R2,R2,#+24 489 ORRS R0,R2 490 B.N _HandleReadRegDone 491_HandleReadRegFPU: 492#if _HAS_FPU_REGS 493 CMP R1,#+53 // if (RegIndex < 53) { (20 (FPSCR), 21-52 FPS0-FPS31) 494 BCS _HandleReadRegDone_Veneer 495 /* 496 Read Coprocessor Access Control Register (CPACR) to check if CP10 and CP11 are enabled 497 If not, access to floating point is not possible 498 CPACR[21:20] == CP10 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved 499 CPACR[23:22] == CP11 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved 500 */ 501 LDR R0,_AddrCPACR 502 LDR R0,[R0] 503 LSLS R0,R0,#+8 504 LSRS R0,R0,#+28 505 CMP R0,#+0xF 506 BEQ _HandleReadRegFPU_Allowed 507 CMP R0,#+0x5 508 BNE _HandleReadRegDone_Veneer 509_HandleReadRegFPU_Allowed: 510 CMP R1,#+21 // if (RegIndex < 21) (20 == FPSCR) 511 BCS _HandleReadRegFPS0_FPS31 512 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 513 BCS _HandleReadFPSCRLazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame 514 LDR R0,=_SYSTEM_FPCCR 515 LDR R0,[R0] 516 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 517 BCS _HandleReadFPSCRLazyMode 518 LDR R0,[R2, #+_APP_SP_OFF_FPSCR] 519 B _HandleReadRegDone 520_HandleReadFPSCRLazyMode: 521 VMRS R0,FPSCR 522 B _HandleReadRegDone 523_HandleReadRegFPS0_FPS31: // RegIndex == 21-52 524 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 525 BCS _HandleReadFPS0_FPS31LazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame 526 LDR R0,=_SYSTEM_FPCCR 527 LDR R0,[R0] 528 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 529 BCS _HandleReadFPS0_FPS31LazyMode 530 SUBS R1,#+21 // Convert absolute reg index into rel. one 531 LSLS R1,R1,#+2 // RegIndex to position on stack 532 ADDS R1,#+_APP_SP_OFF_S0 533 LDR R0,[R2, R1] 534_HandleReadRegDone_Veneer: 535 B _HandleReadRegDone 536_HandleReadFPS0_FPS31LazyMode: 537 SUBS R1,#+20 // convert abs. RegIndex into rel. one 538 MOVS R0,#+6 539 MULS R1,R0,R1 540 LDR R0,=_HandleReadRegUnknown 541 SUB R0,R0,R1 // _HandleReadRegUnknown - 6 * ((RegIndex - 21) + 1) 542 ORR R0,R0,#1 // Thumb bit needs to be set in DestAddr 543 BX R0 544 // 545 // Table for reading FPS0-FPS31 546 // 547 VMOV R0,S31 // v = FPSx 548 B _HandleReadRegDone 549 VMOV R0,S30 550 B _HandleReadRegDone 551 VMOV R0,S29 552 B _HandleReadRegDone 553 VMOV R0,S28 554 B _HandleReadRegDone 555 VMOV R0,S27 556 B _HandleReadRegDone 557 VMOV R0,S26 558 B _HandleReadRegDone 559 VMOV R0,S25 560 B _HandleReadRegDone 561 VMOV R0,S24 562 B _HandleReadRegDone 563 VMOV R0,S23 564 B _HandleReadRegDone 565 VMOV R0,S22 566 B _HandleReadRegDone 567 VMOV R0,S21 568 B _HandleReadRegDone 569 VMOV R0,S20 570 B _HandleReadRegDone 571 VMOV R0,S19 572 B _HandleReadRegDone 573 VMOV R0,S18 574 B _HandleReadRegDone 575 VMOV R0,S17 576 B _HandleReadRegDone 577 VMOV R0,S16 578 B _HandleReadRegDone 579 VMOV R0,S15 580 B _HandleReadRegDone 581 VMOV R0,S14 582 B _HandleReadRegDone 583 VMOV R0,S13 584 B _HandleReadRegDone 585 VMOV R0,S12 586 B _HandleReadRegDone 587 VMOV R0,S11 588 B _HandleReadRegDone 589 VMOV R0,S10 590 B _HandleReadRegDone 591 VMOV R0,S9 592 B _HandleReadRegDone 593 VMOV R0,S8 594 B _HandleReadRegDone 595 VMOV R0,S7 596 B _HandleReadRegDone 597 VMOV R0,S6 598 B _HandleReadRegDone 599 VMOV R0,S5 600 B _HandleReadRegDone 601 VMOV R0,S4 602 B _HandleReadRegDone 603 VMOV R0,S3 604 B _HandleReadRegDone 605 VMOV R0,S2 606 B _HandleReadRegDone 607 VMOV R0,S1 608 B _HandleReadRegDone 609 VMOV R0,S0 610 B _HandleReadRegDone 611#else 612 B _HandleReadRegUnknown 613_HandleReadRegDone_Veneer: 614 B _HandleReadRegDone 615#endif 616_HandleReadRegUnknown: 617 MOVS R0,#+0 // v = 0 618 B.N _HandleReadRegDone 619_HandleReadRegDone: 620 621 // Send register content to J-Link and wait until J-Link has read the data 622 623 STR R0,[R3, #+_SYSTEM_DCRDR_OFF] // DCRDR = v; 624 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Set MON_REQ bit, so J-Link knows monitor is ready to receive commands 625 ORR R0,R0,#0x80000 626 STR R0,[R3, #+_SYSTEM_DEMCR_OFF] // Indicate data ready 627 B _WaitProbeReadIndicateMonRdy 628 629 // Data section for register addresses 630 631_HandleWriteReg: 632 /* 633 _MON_CMD_WRITE_REG 634 CPU: DEMCR[19] -> 1 => Mon ready 635 J-Link: Data -> DCRDR, DEMCR[19] -> 0 => Data placed by probe 636 CPU: DCRDR -> Read, Process command, DEMCR[19] -> 1 => Data read & mon ready 637 638 Register indexes 639 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!) 640 16: XPSR 641 17: MSP 642 18: PSP 643 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] 644 20: FPSCR 645 21-52: FPS0-FPS31 646 647 648 Register usage when entering this "subroutine": 649 R0 Cmd 650 R1 ParamInfo 651 R2 --- 652 R3 = &_SYSTEM_DCRDR (allows also access to DEMCR with offset) 653 R12 --- 654 655 Table B1-9 EXC_RETURN definition of exception return behavior, with FP extension 656 LR Return to Return SP Frame type 657 --------------------------------------------------------- 658 0xFFFFFFE1 Handler mode. MSP Extended 659 0xFFFFFFE9 Thread mode MSP Extended 660 0xFFFFFFED Thread mode PSP Extended 661 0xFFFFFFF1 Handler mode. MSP Basic 662 0xFFFFFFF9 Thread mode MSP Basic 663 0xFFFFFFFD Thread mode PSP Basic 664 665 So LR[2] == 1 => Return stack == PSP else MSP 666 667 R0-R3, R12, PC, xPSR can be written via application stackpointer 668 Other regs can be written directly 669 670 671 Read register data from J-Link into R0 672 */ 673 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] // _SYSTEM_DEMCR |= (1uL << 19); => Monitor is ready to receive register data 674 ORR R0,R0,#0x80000 675 STR R0,[R3, #+_SYSTEM_DEMCR_OFF] 676_HandleWRegWaitUntilDataRecv: 677 LDR R0,[R3, #+_SYSTEM_DEMCR_OFF] 678 LSLS R0,R0,#+12 679 BMI.N _HandleWRegWaitUntilDataRecv // DEMCR[19] == 0 => J-Link has placed new data for us 680 LDR R0,[R3, #+_SYSTEM_DCRDR_OFF] // Get register data 681 // 682 // Determine application SP 683 // 684 LSRS R2,LR,#+3 // Shift LR[2] into carry => Carry clear means that CPU was running on MSP 685 ITE CS 686 MRSCS R2,PSP 687 MRSCC R2,MSP 688 CMP R1,#+4 // if (RegIndex < 4) { (R0-R3) 689 BCS _HandleWriteRegR4 690 STR R0,[R2, R1, LSL #+2] // v = [SP + Rx * 4] (R0-R3) 691 B.N _HandleWriteRegDone 692_HandleWriteRegR4: 693 CMP R1,#+5 // if (RegIndex < 5) { (R4) 694 BCS _HandleWriteRegR5 695 MOV R4,R0 696 B.N _HandleWriteRegDone 697_HandleWriteRegR5: 698 CMP R1,#+6 // if (RegIndex < 6) { (R5) 699 BCS _HandleWriteRegR6 700 MOV R5,R0 701 B.N _HandleWriteRegDone 702_HandleWriteRegR6: 703 CMP R1,#+7 // if (RegIndex < 7) { (R6) 704 BCS _HandleWriteRegR7 705 MOV R6,R0 706 B.N _HandleWriteRegDone 707_HandleWriteRegR7: 708 CMP R1,#+8 // if (RegIndex < 8) { (R7) 709 BCS _HandleWriteRegR8 710 MOV R7,R0 711 B.N _HandleWriteRegDone 712_HandleWriteRegR8: 713 CMP R1,#+9 // if (RegIndex < 9) { (R8) 714 BCS _HandleWriteRegR9 715 MOV R8,R0 716 B.N _HandleWriteRegDone 717_HandleWriteRegR9: 718 CMP R1,#+10 // if (RegIndex < 10) { (R9) 719 BCS _HandleWriteRegR10 720 MOV R9,R0 721 B.N _HandleWriteRegDone 722_HandleWriteRegR10: 723 CMP R1,#+11 // if (RegIndex < 11) { (R10) 724 BCS _HandleWriteRegR11 725 MOV R10,R0 726 B.N _HandleWriteRegDone 727_HandleWriteRegR11: 728 CMP R1,#+12 // if (RegIndex < 12) { (R11) 729 BCS _HandleWriteRegR12 730 MOV R11,R0 731 B.N _HandleWriteRegDone 732_HandleWriteRegR12: 733 CMP R1,#+14 // if (RegIndex < 14) { (R12) 734 BCS _HandleWriteRegR14 735 STR R0,[R2, #+_APP_SP_OFF_R12] 736 B.N _HandleWriteRegDone 737_HandleWriteRegR14: 738 CMP R1,#+15 // if (RegIndex < 15) { (R14 / LR) 739 BCS _HandleWriteRegR15 740 STR R0,[R2, #+_APP_SP_OFF_R14_LR] 741 B.N _HandleWriteRegDone 742_HandleWriteRegR15: 743 CMP R1,#+16 // if (RegIndex < 16) { (R15 / PC) 744 BCS _HandleWriteRegXPSR 745 STR R0,[R2, #+_APP_SP_OFF_PC] 746 B.N _HandleWriteRegDone 747_HandleWriteRegXPSR: 748 CMP R1,#+17 // if (RegIndex < 17) { (xPSR) 749 BCS _HandleWriteRegMSP 750 STR R0,[R2, #+_APP_SP_OFF_XPSR] 751 B.N _HandleWriteRegDone 752_HandleWriteRegMSP: 753 // 754 // For now, SP cannot be modified because it is needed to jump back from monitor mode 755 // 756 CMP R1,#+18 // if (RegIndex < 18) { (MSP) 757 BCS _HandleWriteRegPSP 758 B.N _HandleWriteRegDone 759_HandleWriteRegPSP: // RegIndex == 18 760 CMP R1,#+19 // if (RegIndex < 19) { 761 BCS _HandleWriteRegCFBP 762 B.N _HandleWriteRegDone 763_HandleWriteRegCFBP: 764 /* 765 CFBP is a register that can only be read via debug probe and is a merger of the following regs: 766 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] 767 To keep J-Link side the same for monitor and halt mode, we also return CFBP in monitor mode 768 */ 769 CMP R1,#+20 // if (RegIndex < 20) { (CFBP) 770 BCS _HandleWriteRegFPU 771 LSLS R1,R0,#+24 772 LSRS R1,R1,#+24 // Extract CFBP[7:0] => PRIMASK 773 MSR PRIMASK,R1 774 LSLS R1,R0,#+16 775 LSRS R1,R1,#+24 // Extract CFBP[15:8] => BASEPRI 776 MSR BASEPRI,R1 777 LSLS R1,R0,#+8 // Extract CFBP[23:16] => FAULTMASK 778 LSRS R1,R1,#+24 779 MSR FAULTMASK,R1 780 LSRS R1,R0,#+24 // Extract CFBP[31:24] => CONTROL 781 LSRS R0,R1,#2 // Current CONTROL[1] -> Carry 782 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 783 ORRCS LR,LR,#+4 784 BICCC LR,LR,#+4 785 BIC R1,R1,#+2 // CONTROL.SPSEL (CONTROL[1]) == 0 inside monitor. Otherwise behavior is UNPREDICTABLE 786 LSRS R0,R1,#+3 // New CONTROL.FPCA (CONTROL[2]) -> Carry 787 ITE CS // CONTROL[2] == FPCA => NOT(FPCA) saved to LR[4]. LR[4] == 0 => Extended stack frame, so FPU regs possibly on stack 788 BICCS LR,LR,#+0x10 // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame 789 ORRCC LR,LR,#+0x10 790 MRS R0,CONTROL 791 LSRS R0,R0,#+3 // CONTROL[2] -> Carry 792 ITE CS // Preserve original value of current CONTROL[2] 793 ORRCS R1,R1,#+0x04 794 BICCC R1,R1,#+0x04 795 MSR CONTROL,R1 796 ISB // Necessary after writing to CONTROL, see ARM DDI0403D, B1.4.4 The special-purpose CONTROL register 797 B.N _HandleWriteRegDone 798_HandleWriteRegFPU: 799#if _HAS_FPU_REGS 800 CMP R1,#+53 // if (RegIndex < 53) { (20 (FPSCR), 21-52 FPS0-FPS31) 801 BCS _HandleWriteRegDone_Veneer 802 /* 803 Read Coprocessor Access Control Register (CPACR) to check if CP10 and CP11 are enabled 804 If not, access to floating point is not possible 805 CPACR[21:20] == CP10 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved 806 CPACR[23:22] == CP11 enable. 0b01 = Privileged access only. 0b11 = Full access. Other = reserved 807 */ 808 MOV R12,R0 // Save register data 809 LDR R0,_AddrCPACR 810 LDR R0,[R0] 811 LSLS R0,R0,#+8 812 LSRS R0,R0,#+28 813 CMP R0,#+0xF 814 BEQ _HandleWriteRegFPU_Allowed 815 CMP R0,#+0x5 816 BNE _HandleWriteRegDone_Veneer 817_HandleWriteRegFPU_Allowed: 818 CMP R1,#+21 // if (RegIndex < 21) (20 == FPSCR) 819 BCS _HandleWriteRegFPS0_FPS31 820 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 821 BCS _HandleWriteFPSCRLazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame 822 LDR R0,=_SYSTEM_FPCCR 823 LDR R0,[R0] 824 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 825 BCS _HandleWriteFPSCRLazyMode 826 STR R12,[R2, #+_APP_SP_OFF_FPSCR] 827 B _HandleWriteRegDone 828_HandleWriteFPSCRLazyMode: 829 VMSR FPSCR,R12 830 B _HandleWriteRegDone 831_HandleWriteRegFPS0_FPS31: // RegIndex == 21-52 832 LDR R0,=_SYSTEM_FPCCR 833 LDR R0,[R0] 834 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 835 BCS _HandleWriteFPS0_FPS31LazyMode 836 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 837 BCS _HandleWriteFPS0_FPS31LazyMode // Remember: NOT(FPCA) is stored to LR. == 0 means: Extended stack frame 838 SUBS R1,#+21 // Convert absolute reg index into rel. one 839 LSLS R1,R1,#+2 // RegIndex to position on stack 840 ADDS R1,#+_APP_SP_OFF_S0 841 STR R12,[R2, R1] 842_HandleWriteRegDone_Veneer: 843 B _HandleWriteRegDone 844_HandleWriteFPS0_FPS31LazyMode: 845 SUBS R1,#+20 // Convert abs. RegIndex into rel. one 846 MOVS R0,#+6 847 MULS R1,R0,R1 848 LDR R0,=_HandleReadRegUnknown 849 SUB R0,R0,R1 // _HandleReadRegUnknown - 6 * ((RegIndex - 21) + 1) 850 ORR R0,R0,#1 // Thumb bit needs to be set in DestAddr 851 BX R0 852 // 853 // Table for reading FPS0-FPS31 854 // 855 VMOV S31,R12 // v = FPSx 856 B _HandleWriteRegDone 857 VMOV S30,R12 858 B _HandleWriteRegDone 859 VMOV S29,R12 860 B _HandleWriteRegDone 861 VMOV S28,R12 862 B _HandleWriteRegDone 863 VMOV S27,R12 864 B _HandleWriteRegDone 865 VMOV S26,R12 866 B _HandleWriteRegDone 867 VMOV S25,R12 868 B _HandleWriteRegDone 869 VMOV S24,R12 870 B _HandleWriteRegDone 871 VMOV S23,R12 872 B _HandleWriteRegDone 873 VMOV S22,R12 874 B _HandleWriteRegDone 875 VMOV S21,R12 876 B _HandleWriteRegDone 877 VMOV S20,R12 878 B _HandleWriteRegDone 879 VMOV S19,R12 880 B _HandleWriteRegDone 881 VMOV S18,R12 882 B _HandleWriteRegDone 883 VMOV S17,R12 884 B _HandleWriteRegDone 885 VMOV S16,R12 886 B _HandleWriteRegDone 887 VMOV S15,R12 888 B _HandleWriteRegDone 889 VMOV S14,R12 890 B _HandleWriteRegDone 891 VMOV S13,R12 892 B _HandleWriteRegDone 893 VMOV S12,R12 894 B _HandleWriteRegDone 895 VMOV S11,R12 896 B _HandleWriteRegDone 897 VMOV S10,R12 898 B _HandleWriteRegDone 899 VMOV S9,R12 900 B _HandleWriteRegDone 901 VMOV S8,R12 902 B _HandleWriteRegDone 903 VMOV S7,R12 904 B _HandleWriteRegDone 905 VMOV S6,R12 906 B _HandleWriteRegDone 907 VMOV S5,R12 908 B _HandleWriteRegDone 909 VMOV S4,R12 910 B _HandleWriteRegDone 911 VMOV S3,R12 912 B _HandleWriteRegDone 913 VMOV S2,R12 914 B _HandleWriteRegDone 915 VMOV S1,R12 916 B _HandleWriteRegDone 917 VMOV S0,R12 918 B _HandleWriteRegDone 919#else 920 B _HandleWriteRegUnknown 921#endif 922_HandleWriteRegUnknown: 923 B.N _HandleWriteRegDone 924_HandleWriteRegDone: 925 B _IndicateMonReady // Indicate that monitor has read data, processed command and is ready for a new one 926 .end 927/****** End Of File *************************************************/ 928
