/*************************************************************************** * Copyright (C) 2006 by Dominic Rath * * Dominic.Rath@gmx.de * * * * This program is free software; you can redistribute it and/or modify * * it under the terms of the GNU General Public License as published by * * the Free Software Foundation; either version 2 of the License, or * * (at your option) any later version. * * * * This program is distributed in the hope that it will be useful, * * but WITHOUT ANY WARRANTY; without even the implied warranty of * * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the * * GNU General Public License for more details. * * * * You should have received a copy of the GNU General Public License * * along with this program; if not, write to the * * Free Software Foundation, Inc., * * 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. * ***************************************************************************/ #include "protocol.h" .text .align 4 @ Disable thumb mode .code 32 @ send word to debugger .macro m_send_to_debugger reg 1: mrc p14, 0, r15, c14, c0, 0 bvs 1b mcr p14, 0, \reg, c8, c0, 0 .endm @ receive word from debugger .macro m_receive_from_debugger reg 1: mrc p14, 0, r15, c14, c0, 0 bpl 1b mrc p14, 0, \reg, c9, c0, 0 .endm @ save register on debugger, small .macro m_small_save_reg reg mov r0, \reg bl send_to_debugger .endm @ save status register on debugger, small .macro m_small_save_psr mrs r0, spsr bl send_to_debugger .endm @ wait for all outstanding coprocessor accesses to complete .macro m_cpwait mrc p15, 0, r0, c2, c0, 0 mov r0, r0 sub pc, pc, #4 .endm .global reset_handler .global undef_handler .global swi_handler .global prefetch_abort_handler .global data_abort_handler .global irq_handler .global fiq_handler .section .part1 , "ax" reset_handler: @ read DCSR mrc p14, 0, r13, c10, c0 @ check if global enable bit (GE) is set ands r13, r13, #0x80000000 bne debug_handler @ set global enable bit (GE) mov r13, #0xc0000000 mcr p14, 0, r13, c10, c0 debug_handler: @ save r0 without modifying other registers m_send_to_debugger r0 @ save lr (program PC) without branching (use macro) m_send_to_debugger r14 @ save non-banked registers and spsr (program CPSR) m_small_save_reg r1 m_small_save_reg r2 m_small_save_reg r3 m_small_save_reg r4 m_small_save_reg r5 m_small_save_reg r6 m_small_save_reg r7 m_small_save_psr mrs r0, spsr @ prepare program PSR for debug use (clear Thumb, set I/F to disable interrupts) bic r0, r0, #PSR_T orr r0, r0, #(PSR_I | PSR_F) @ examine mode bits and r1, r0, #MODE_MASK cmp r1, #MODE_USR bne not_user_mode @ replace USR mode with SYS bic r0, r0, #MODE_MASK orr r0, r0, #MODE_SYS not_user_mode: b save_banked_registers @ command loop @ wait for command from debugger, than execute desired function get_command: bl receive_from_debugger @ 0x0n - register access cmp r0, #0x0 beq get_banked_registers cmp r0, #0x1 beq set_banked_registers @ 0x1n - read memory cmp r0, #0x11 beq read_byte cmp r0, #0x12 beq read_half_word cmp r0, #0x14 beq read_word @ 0x2n - write memory cmp r0, #0x21 beq write_byte cmp r0, #0x22 beq write_half_word cmp r0, #0x24 beq write_word @ 0x3n - program execution cmp r0, #0x30 beq resume cmp r0, #0x31 beq resume_w_trace @ 0x4n - coprocessor access cmp r0, #0x40 beq read_cp_reg cmp r0, #0x41 beq write_cp_reg @ 0x5n - cache and mmu functions cmp r0, #0x50 beq clean_d_cache cmp r0, #0x51 beq invalidate_d_cache cmp r0, #0x52 beq invalidate_i_cache cmp r0, #0x53 beq cpwait @ 0x6n - misc functions cmp r0, #0x60 beq clear_sa cmp r0, #0x61 beq read_trace_buffer cmp r0, #0x62 beq clean_trace_buffer @ return (back to get_command) b get_command @ ---- @ resume program execution resume: @ restore CPSR (SPSR_dbg) bl receive_from_debugger msr spsr, r0 @ restore registers (r7 - r0) bl receive_from_debugger @ r7 mov r7, r0 bl receive_from_debugger @ r6 mov r6, r0 bl receive_from_debugger @ r5 mov r5, r0 bl receive_from_debugger @ r4 mov r4, r0 bl receive_from_debugger @ r3 mov r3, r0 bl receive_from_debugger @ r2 mov r2, r0 bl receive_from_debugger @ r1 mov r1, r0 bl receive_from_debugger @ r0 @ resume addresss m_receive_from_debugger lr @ branch back to application code, restoring CPSR subs pc, lr, #0 @ get banked registers @ receive mode bits from host, then run into save_banked_registers to get_banked_registers: bl receive_from_debugger @ save banked registers @ r0[4:0]: desired mode bits save_banked_registers: @ backup CPSR mrs r7, cpsr msr cpsr_c, r0 nop @ keep current mode bits in r1 for later use and r1, r0, #MODE_MASK @ backup banked registers m_send_to_debugger r8 m_send_to_debugger r9 m_send_to_debugger r10 m_send_to_debugger r11 m_send_to_debugger r12 m_send_to_debugger r13 m_send_to_debugger r14 @ if not in SYS mode (or USR, which we replaced with SYS before) cmp r1, #MODE_SYS beq no_spsr_to_save @ backup SPSR mrs r0, spsr m_send_to_debugger r0 no_spsr_to_save: @ restore CPSR for SDS msr cpsr_c, r7 nop @ return b get_command @ ---- @ set banked registers @ receive mode bits from host, then run into save_banked_registers to set_banked_registers: bl receive_from_debugger @ restore banked registers @ r0[4:0]: desired mode bits restore_banked_registers: @ backup CPSR mrs r7, cpsr msr cpsr_c, r0 nop @ keep current mode bits in r1 for later use and r1, r0, #MODE_MASK @ set banked registers m_receive_from_debugger r8 m_receive_from_debugger r9 m_receive_from_debugger r10 m_receive_from_debugger r11 m_receive_from_debugger r12 m_receive_from_debugger r13 m_receive_from_debugger r14 @ if not in SYS mode (or USR, which we replaced with SYS before) cmp r1, #MODE_SYS beq no_spsr_to_restore @ set SPSR m_receive_from_debugger r0 msr spsr, r0 no_spsr_to_restore: @ restore CPSR for SDS msr cpsr_c, r7 nop @ return b get_command @ ---- read_byte: @ r2: address bl receive_from_debugger mov r2, r0 @ r1: count bl receive_from_debugger mov r1, r0 rb_loop: ldrb r0, [r2], #1 @ drain write- (and fill-) buffer to work around XScale errata mcr p15, 0, r8, c7, c10, 4 bl send_to_debugger subs r1, r1, #1 bne rb_loop @ return b get_command @ ---- read_half_word: @ r2: address bl receive_from_debugger mov r2, r0 @ r1: count bl receive_from_debugger mov r1, r0 rh_loop: ldrh r0, [r2], #2 @ drain write- (and fill-) buffer to work around XScale errata mcr p15, 0, r8, c7, c10, 4 bl send_to_debugger subs r1, r1, #1 bne rh_loop @ return b get_command @ ---- read_word: @ r2: address bl receive_from_debugger mov r2, r0 @ r1: count bl receive_from_debugger mov r1, r0 rw_loop: ldr r0, [r2], #4 @ drain write- (and fill-) buffer to work around XScale errata mcr p15, 0, r8, c7, c10, 4 bl send_to_debugger subs r1, r1, #1 bne rw_loop @ return b get_command @ ---- write_byte: @ r2: address bl receive_from_debugger mov r2, r0 @ r1: count bl receive_from_debugger mov r1, r0 wb_loop: bl receive_from_debugger strb r0, [r2], #1 @ drain write- (and fill-) buffer to work around XScale errata mcr p15, 0, r8, c7, c10, 4 subs r1, r1, #1 bne wb_loop @ return b get_command @ ---- write_half_word: @ r2: address bl receive_from_debugger mov r2, r0 @ r1: count bl receive_from_debugger mov r1, r0 wh_loop: bl receive_from_debugger strh r0, [r2], #2 @ drain write- (and fill-) buffer to work around XScale errata mcr p15, 0, r8, c7, c10, 4 subs r1, r1, #1 bne wh_loop @ return b get_command @ ---- write_word: @ r2: address bl receive_from_debugger mov r2, r0 @ r1: count bl receive_from_debugger mov r1, r0 ww_loop: bl receive_from_debugger str r0, [r2], #4 @ drain write- (and fill-) buffer to work around XScale errata mcr p15, 0, r8, c7, c10, 4 subs r1, r1, #1 bne ww_loop @ return b get_command @ ---- clear_sa: @ read DCSR mrc p14, 0, r0, c10, c0 @ clear SA bit bic r0, r0, #0x20 @ write DCSR mcr p14, 0, r0, c10, c0 @ return b get_command @ ---- clean_d_cache: @ r0: cache clean area bl receive_from_debugger mov r1, #1024 clean_loop: mcr p15, 0, r0, c7, c2, 5 add r0, r0, #32 subs r1, r1, #1 bne clean_loop @ return b get_command @ ---- invalidate_d_cache: mcr p15, 0, r0, c7, c6, 0 @ return b get_command @ ---- invalidate_i_cache: mcr p15, 0, r0, c7, c5, 0 @ return b get_command @ ---- cpwait: m_cpwait @return b get_command @ ---- .section .part2 , "ax" read_cp_reg: @ requested cp register bl receive_from_debugger adr r1, read_cp_table add pc, r1, r0, lsl #3 read_cp_table: mrc p15, 0, r0, c0, c0, 0 @ XSCALE_MAINID b read_cp_reg_reply mrc p15, 0, r0, c0, c0, 1 @ XSCALE_CACHETYPE b read_cp_reg_reply mrc p15, 0, r0, c1, c0, 0 @ XSCALE_CTRL b read_cp_reg_reply mrc p15, 0, r0, c1, c0, 1 @ XSCALE_AUXCTRL b read_cp_reg_reply mrc p15, 0, r0, c2, c0, 0 @ XSCALE_TTB b read_cp_reg_reply mrc p15, 0, r0, c3, c0, 0 @ XSCALE_DAC b read_cp_reg_reply mrc p15, 0, r0, c5, c0, 0 @ XSCALE_FSR b read_cp_reg_reply mrc p15, 0, r0, c6, c0, 0 @ XSCALE_FAR b read_cp_reg_reply mrc p15, 0, r0, c13, c0, 0 @ XSCALE_PID b read_cp_reg_reply mrc p15, 0, r0, c15, c0, 0 @ XSCALE_CP_ACCESS b read_cp_reg_reply mrc p15, 0, r0, c14, c8, 0 @ XSCALE_IBCR0 b read_cp_reg_reply mrc p15, 0, r0, c14, c9, 0 @ XSCALE_IBCR1 b read_cp_reg_reply mrc p15, 0, r0, c14, c0, 0 @ XSCALE_DBR0 b read_cp_reg_reply mrc p15, 0, r0, c14, c3, 0 @ XSCALE_DBR1 b read_cp_reg_reply mrc p15, 0, r0, c14, c4, 0 @ XSCALE_DBCON b read_cp_reg_reply mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG b read_cp_reg_reply mrc p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 b read_cp_reg_reply mrc p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 b read_cp_reg_reply mrc p14, 0, r0, c10, c0, 0 @ XSCALE_DCSR b read_cp_reg_reply read_cp_reg_reply: bl send_to_debugger @ return b get_command @ ---- write_cp_reg: @ requested cp register bl receive_from_debugger mov r1, r0 @ value to be written bl receive_from_debugger adr r2, write_cp_table add pc, r2, r1, lsl #3 write_cp_table: mcr p15, 0, r0, c0, c0, 0 @ XSCALE_MAINID (0x0) b get_command mcr p15, 0, r0, c0, c0, 1 @ XSCALE_CACHETYPE (0x1) b get_command mcr p15, 0, r0, c1, c0, 0 @ XSCALE_CTRL (0x2) b get_command mcr p15, 0, r0, c1, c0, 1 @ XSCALE_AUXCTRL (0x3) b get_command mcr p15, 0, r0, c2, c0, 0 @ XSCALE_TTB (0x4) b get_command mcr p15, 0, r0, c3, c0, 0 @ XSCALE_DAC (0x5) b get_command mcr p15, 0, r0, c5, c0, 0 @ XSCALE_FSR (0x6) b get_command mcr p15, 0, r0, c6, c0, 0 @ XSCALE_FAR (0x7) b get_command mcr p15, 0, r0, c13, c0, 0 @ XSCALE_PID (0x8) b get_command mcr p15, 0, r0, c15, c0, 0 @ XSCALE_CP_ACCESS (0x9) b get_command mcr p15, 0, r0, c14, c8, 0 @ XSCALE_IBCR0 (0xa) b get_command mcr p15, 0, r0, c14, c9, 0 @ XSCALE_IBCR1 (0xb) b get_command mcr p15, 0, r0, c14, c0, 0 @ XSCALE_DBR0 (0xc) b get_command mcr p15, 0, r0, c14, c3, 0 @ XSCALE_DBR1 (0xd) b get_command mcr p15, 0, r0, c14, c4, 0 @ XSCALE_DBCON (0xe) b get_command mcr p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG (0xf) b get_command mcr p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 (0x10) b get_command mcr p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 (0x11) b get_command mcr p14, 0, r0, c10, c0, 0 @ XSCALE_DCSR (0x12) b get_command @ ---- read_trace_buffer: @ dump 256 entries from trace buffer mov r1, #256 read_tb_loop: mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG bl send_to_debugger subs r1, r1, #1 bne read_tb_loop @ dump checkpoint register 0 mrc p14, 0, r0, c12, c0, 0 @ XSCALE_CHKPT0 (0x10) bl send_to_debugger @ dump checkpoint register 1 mrc p14, 0, r0, c13, c0, 0 @ XSCALE_CHKPT1 (0x11) bl send_to_debugger @ return b get_command @ ---- clean_trace_buffer: @ clean 256 entries from trace buffer mov r1, #256 clean_tb_loop: mrc p14, 0, r0, c11, c0, 0 @ XSCALE_TBREG subs r1, r1, #1 bne clean_tb_loop @ return b get_command @ ---- @ resume program execution with trace buffer enabled resume_w_trace: @ restore CPSR (SPSR_dbg) bl receive_from_debugger msr spsr, r0 @ restore registers (r7 - r0) bl receive_from_debugger @ r7 mov r7, r0 bl receive_from_debugger @ r6 mov r6, r0 bl receive_from_debugger @ r5 mov r5, r0 bl receive_from_debugger @ r4 mov r4, r0 bl receive_from_debugger @ r3 mov r3, r0 bl receive_from_debugger @ r2 mov r2, r0 bl receive_from_debugger @ r1 mov r1, r0 bl receive_from_debugger @ r0 @ resume addresss m_receive_from_debugger lr mrc p14, 0, r13, c10, c0, 0 @ XSCALE_DCSR orr r13, r13, #1 mcr p14, 0, r13, c10, c0, 0 @ XSCALE_DCSR @ branch back to application code, restoring CPSR subs pc, lr, #0 undef_handler: swi_handler: prefetch_abort_handler: data_abort_handler: irq_handler: fiq_handler: 1: b 1b send_to_debugger: m_send_to_debugger r0 mov pc, lr receive_from_debugger: m_receive_from_debugger r0 mov pc, lr