--- /dev/null
+/***************************************************************************
+ * 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
+
Code Review / openocd.git / blobdiff