tinyriscv-openocd/contrib/loaders/debug/xscale/debug_handler.S

717 lines
13 KiB
ArmAsm

/***************************************************************************
* 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, see <http://www.gnu.org/licenses/>. *
***************************************************************************/
#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