start: LDR X15, addr_debugger_storage STR X0, [X15, #0] STR X1, [X15, #8] STR X2, [X15, #16] STR X3, [X15, #24] STR X4, [X15, #32] STR X5, [X15, #40] STR X6, [X15, #48] STR X7, [X15, #56] STR X8, [X15, #64] STR X9, [X15, #72] STR X10, [X15, #80] STR X11, [X15, #88] STR X12, [X15, #96] STR X13, [X15, #104] STR X14, [X15, #112] STR X15, [X15, #120] STR X16, [X15, #128] STR X17, [X15, #136] STR X18, [X15, #144] STR X19, [X15, #152] STR X20, [X15, #160] STR X21, [X15, #168] STR X22, [X15, #176] STR X23, [X15, #184] STR X24, [X15, #192] STR X25, [X15, #200] STR X26, [X15, #208] STR X27, [X15, #216] STR X28, [X15, #224] STR X29, [X15, #232] STR X30, [X15, #240] MOV X0, SP STR X0, [X15, #248] // Does not override the SP etc. TODO ELHER make configurable in debugger (during setup) // B debugger_main // Overwrite SP and FP with the debugger stack location LDR X0, addr_debugger_stack MOV SP, X0 MOV FP, X0 // See if we need to disable the MMU on entry // LDR X0, [X15, #0xfd0] // CMP X0, #0x777 //Disable the MMU // BL disable_mmu // Compare to see if we need to jump into the debugger or need to continue the 'normal' execution flow LDR X0, [X15, #4064] CMP X0, #0x777 B.NE debugger_main # Continue 'normal' execution flow BL sync_processor_state // Restore LR and FP LDR X30, [X15, #240] LDR X29, [X15, #232] // Restore the stack pointer LDR X0, [X15, #248] MOV SP, X0 // Also restore X0 LDR X0, [X15, #0] LDR X15, [X15, #0xfd8] BR X15 .text .global disable_mmu disable_mmu: MRS X0, SCTLR_EL3 AND X0, X0, #-0x2 RET .text .global enable_mmu enable_mmu: // TODO crashes on EL1? MRS X0, SCTLR_EL3 ORR X0, X0, #0x1 RET .text .global restore_and_jump restore_and_jump: BL sync_processor_state //See if we need to enable the MMU // LDR X0, [X15, #0xfd0] // CMP X0, #0x777 //Disable the MMU // BL enable_mmu // Restore LR and FP LDR X30, [X15, #240] LDR X29, [X15, #232] // Restore the stack pointer LDR X0, [X15, #248] MOV SP, X0 // Also restore X0 LDR X0, [X15, #0] // Load target address and branch to it without setting the LR // JUMP_ADDR LDR X15, [X15, #4088] BR X15 .text .global dump_special_el3 dump_special_el3: MRS X0, TTBR0_EL3 STR X0, [X15, #256] MRS X0, SCTLR_EL3 STR X0, [X15, #280] MRS X0, VBAR_EL3 STR X0, [X15, #304] MRS X0, TCR_EL3 STR X0, [X15, #328] MRS X0, ELR_EL3 STR X0, [X15, #352] MRS X0, SPSR_EL3 STR X0, [X15, #400] MRS X0, MAIR_EL3 STR X0, [X15, #424] // Also dump el2, el1 BL dump_special_el2 BL dump_special_el1 RET .text .global dump_special_el2 dump_special_el2: MRS X0, TTBR0_EL2 STR X0, [X15, #264] MRS X0, SCTLR_EL2 STR X0, [X15, #288] MRS X0, VBAR_EL2 STR X0, [X15, #312] MRS X0, TCR_EL2 STR X0, [X15, #336] MRS X0, ELR_EL2 STR X0, [X15, #360] MRS X0, SP_EL2 STR X0, [X15, #376] MRS X0, SPSR_EL2 STR X0, [X15, #408] MRS X0, MAIR_EL2 STR X0, [X15, #432] // also dump EL1 BL dump_special_el1 RET .text .global dump_special_el1 dump_special_el1: // EL1 registers dump MRS X0, TTBR0_EL1 STR X0, [X15, #272] MRS X0, SCTLR_EL1 STR X0, [X15, #296] MRS X0, VBAR_EL1 STR X0, [X15, #320] MRS X0, TCR_EL1 STR X0, [X15, #344] MRS X0, ELR_EL1 STR X0, [X15, #368] // see https://community.arm.com/support-forums/f/architectures-and-processors-forum/49184/difference-between-sp_el1-and-spsel-mov // TODO for dumping with interactive shellcode? // MRS X0, SP_EL1 // STR X0, [X15, #384] MRS X0, SPSR_EL1 STR X0, [X15, #416] MRS X0, MAIR_EL1 STR X0, [X15, #440] RET .text .global dump_special_regs dump_special_regs: LDR X15, addr_debugger_storage MRS X0, CurrentEL STR X0, [X15, #448] cmp X0, #0b1100 // EL3 BEQ dump_special_el3 cmp X0, #0b1000 // EL2 BEQ dump_special_el2 cmp X0, #0b0100 // EL1 BEQ dump_special_el1 // EL0 registers MRS X0, SP_EL0 STR X0, [X15, #392] RET .text .global write_special_el3 write_special_el3: LDR X0, [X15, #256] MSR TTBR0_EL3, X0 LDR X0, [X15, #280] MSR SCTLR_EL3, X0 LDR X0, [X15, #304] MSR VBAR_EL3, X0 LDR X0, [X15, #328] MSR TCR_EL3, X0 LDR X0, [X15, #352] MSR ELR_EL3, X0 LDR X0, [X15, #376] MSR SP_EL2, X0 LDR X0, [X15, #400] MSR SPSR_EL3, X0 LDR X0, [X15, #424] MSR MAIR_EL3, X0 BL write_special_el2 BL write_special_el1 RET .text .global write_special_el2 write_special_el2: LDR X0, [X15, #264] MSR TTBR0_EL2, X0 LDR X0, [X15, #288] MSR SCTLR_EL2, X0 LDR X0, [X15, #312] MSR VBAR_EL2, X0 LDR X0, [X15, #336] MSR TCR_EL2, X0 LDR X0, [X15, #360] MSR ELR_EL2, X0 LDR X0, [X15, #384] MSR SP_EL1, X0 LDR X0, [X15, #408] MSR SPSR_EL2, X0 LDR X0, [X15, #432] MSR MAIR_EL2, X0 BL write_special_el1 RET .text .global write_special_el1 write_special_el1: LDR X0, [X15, #272] MSR TTBR0_EL1, X0 LDR X0, [X15, #296] MSR SCTLR_EL1, X0 LDR X0, [X15, #320] MSR VBAR_EL1, X0 LDR X0, [X15, #344] MSR TCR_EL1, X0 LDR X0, [X15, #368] MSR ELR_EL1, X0 LDR X0, [X15, #392] MSR SP_EL0, X0 LDR X0, [X15, #416] MSR SPSR_EL1, X0 LDR X0, [X15, #440] MSR MAIR_EL1, X0 RET .text .global write_special_regs write_special_regs: LDR X15, addr_debugger_storage MRS X0, CurrentEL STR X0, [X15, #448] cmp X0, #0b1100 // EL3 BEQ write_special_el3 cmp X0, #0b1000 // EL2 BEQ write_special_el2 cmp X0, #0b0100 // EL1 BEQ write_special_el1 RET .text .global debugger_sync_special_regs debugger_sync_special_regs: BL write_special_regs B debugger_main .text .global debugger_dump_special_regs debugger_dump_special_regs: BL dump_special_regs B debugger_main .text .global sync_processor_state sync_processor_state: //Corrupt X15 to use it to restore the rest of the state, except for the SP, LR and FP LDR X15, addr_debugger_storage LDR X1, [X15, #8] LDR X2, [X15, #16] LDR X3, [X15, #24] LDR X4, [X15, #32] LDR X5, [X15, #40] LDR X6, [X15, #48] LDR X7, [X15, #56] LDR X8, [X15, #64] LDR X9, [X15, #72] LDR X10, [X15, #80] LDR X11, [X15, #88] LDR X12, [X15, #96] LDR X13, [X15, #104] LDR X14, [X15, #112] LDR X15, [X15, #120] LDR X16, [X15, #128] LDR X17, [X15, #136] LDR X18, [X15, #144] LDR X19, [X15, #152] LDR X20, [X15, #160] LDR X21, [X15, #168] LDR X22, [X15, #176] LDR X23, [X15, #184] LDR X24, [X15, #192] LDR X25, [X15, #200] LDR X26, [X15, #208] LDR X27, [X15, #216] LDR X28, [X15, #224] RET .text .global restore_and_return restore_and_return: B sync_processor_state // Restore LR and FP LDR X30, [X15, #240] LDR X29, [X15, #232] // Restore the stack pointer LDR X0, [X15, #248] MOV SP, X0 // Also restore X0 LDR X0, [X15, #0] RET .text .global sync_debugger sync_debugger: BL sync_processor_state B debugger_main .align 3 addr_debugger_storage: .quad debugger_storage addr_debugger_stack: .quad debugger_stack