debug_handler.s

linux下的jtag调试软件

/*
 * This file is part of Jelie,
 * (c) 2002 Julien Pilet <[e-mail]> and
 * Stephane Magnenat <[e-mail]>
 *
 * Jelie 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.
 *
 * Jelie 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 Foobar; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */

@
@ Debug Handler for the PXA250
@

#include "pxa250regs.S"
#include "target_to_host.h"

.macro CPWAIT
	mrc p15, 0, r0, c2, c0, 0	@ arbitrary read of cp15
	mov r0, r0
	sub pc, pc, #4			@ branch to next instruction

	@ at this point, any previous cp15 writes are
	@ guaranteed to have taken effect.
.endm

@ Blocking macro to read a value sent by the debugger.
.macro readrx_macro reg
1:
	@ Read the RR bit of TXRXCTRL into flags
	mrc p14, 0, r15, c14, c0, 0
	@ if RX is not valid, loop
	bpl 1b
	@ if RX is valid, read it
	mrc p14, 0, \reg, c9, c0, 0
.endm

@ Blocking macro to sent a word to the debugger
.macro writetx_macro reg
1:
	@ poll the TR bit of TXRXCTRL into flags
	mrc p14, 0, r15, c14, c0, 0
	bvs 1b

	@ write data into tx
	mcr p14, 0, \reg, c8, c0, 0
.endm


@ Macro used to save a register on the debugger, without modifying 
@ any register. But its quite big.
.macro save_reg r, code
1:
	@ poll the TR bit of TXRXCTRL into flags
	mrc p14, 0, r15, c14, c0, 0		@ this clears the r13 register !!
	bvs 1b

	mov sp, #\code
	add sp, sp, #SAVE_ON_HOST	@ its a SAVE command

	@ send the SAVE command
	mcr p14, 0, sp, c8, c0, 0

	writetx_macro \r	@ send the value
.endm

@ Macro used to load a register from host, without modifying any other
@ register.
.macro restore_reg r, code
1:
	@ poll the TR bit of TXRXCTRL into flags
	mrc p14, 0, r15, c14, c0, 0		@ this clears the r13 register !!
	bvs 1b

	mov sp, #\code
	orr sp, sp, #LOAD_FROM_HOST	@ its a LOAD command

	@ send the LOAD command
	mcr p14, 0, sp, c8, c0, 0

	readrx_macro \r		@ read the value from host
.endm

.macro GetWordFromHost
	bl readrx
.endm

.macro SendWordToHost
	bl writetx
.endm


.text
.code 32


@ save_on_host:
@ In:	r0 - value to save
@	r1 - place number
@ Out: -
@ Mod: lr

save_on_host:
	orr r1, r1, #SAVE_ON_HOST	@ it s a SAVE command
	writetx_macro r1	@ send the command
	writetx_macro r0	@ send the value
	mov pc, lr

.macro short_save_reg r, place
	mov r1, #\place
	mov r0, \r
	bl save_on_host
.endm

@ load_from_host:
@ In:	r0 - place number
@ Out:	r0 - the loaded value
@ Mod:	-
load_from_host:
	orr r0, r0, #LOAD_FROM_HOST	@ make the LOAD command code
	writetx_macro r0		@ send the command
	readrx_macro r0			@ read the answer, store in r0
	mov pc, lr			@ return

.macro short_restore_reg r, place
	mov r0, #\place
	bl load_from_host
	mov \r, r0
.endm

@ This function handles debug events.
@ It s in the Special Debug State. ldm and stm are forbidden,
@ a nop should be added after each memory access.

.global DebugHandler
DebugHandler:
	@ Save registers in memory
	save_reg r0, save_R0
	save_reg r1, save_R1
	sub lr, lr, #4
	save_reg lr, save_LR	@ LR contains the PC+4 of the running program
	short_save_reg r2, save_R2
	short_save_reg r3, save_R3
	short_save_reg r4, save_R4
	short_save_reg r5, save_R5
	short_save_reg r6, save_R6
	short_save_reg r7, save_R7
	short_save_reg r8, save_R8
	short_save_reg r9, save_R9
	short_save_reg r10, save_R10
	short_save_reg r11, save_R11
	short_save_reg r12, save_R12

	mrs r0, spsr		@ give the SPSR to the debugger
	mov r1, #save_SPSR	
	bl save_on_host

	@ try to get R13 and R14. 
	mrs r4, cpsr		@ backup the current PSR

	and r1, r0, #MODE_MASK	@ Intel says we can t switch
	cmp r1, #USR_MODE	@ to user mode. But we can use the system mode instead.

	bne save_no_system_mode	

	bic r5, r5, #MODE_MASK
	orr r5, r5, #SYSTEM_MODE	@ replace the user mode by the SYSTEM mode

	save_no_system_mode:

	msr cpsr_c, r0		@ switch to the debugged program mode
	nop
	mov r0, r13
	mov r2, r14

	msr cpsr_c, r4		@ switch back to debug mode
	nop

	mov r1, #save_R13
	bl save_on_host		@ save R13

	mov r0, r2
	mov r1, #save_R14
	bl save_on_host		@ save R14

WaitForCommand:
	@ tell the host we are ready
	mov r0, #'?'
	orr r0, r0, #TARGET_READY_FOR_CMD
	SendWordToHost		

	GetWordFromHost		@ wait for the answer
	
	adr lr, WaitForCommand	@ setup the link register manually, to jump back
				@ automatically after each command to
				@ WaitForCommand.

	cmp r0, #'c'
	beq continue		@ link reg is not used, cause it ll never return

	cmp r0, #'g'
	beq SendToHost

	cmp r0, #'p'
	beq ReceiveFromHost

	cmp r0, #'b'
	beq SetBreakpoint

	SendWordToHost

	mov r0, #INVALID_COMMAND
	SendWordToHost
	b WaitForCommand

continue:
	@ first: invalidate instruction cache, in case the debug handler 
	@ has modified the memory.

	mcr p15, 0, r1, c7, c5, 0

	CPWAIT	@ see User s Manual, section 2.3.3
	
	@ restore regisers
	
	mov r0, #save_SPSR
	bl load_from_host
	mov r5, r0		@ copy the CPSR of the debugged prog to r5
	msr spsr_cxsf, r0

	@ try to restore R13 and R14. 
	mrs r2, cpsr		@ backup the current PSR

	and r1, r0, #MODE_MASK	@ Intel says we can t switch
	cmp r1, #USR_MODE	@ to user mode. But we can use the system mode instead.

	bne no_system_mode	

	bic r5, r5, #MODE_MASK
	orr r5, r5, #SYSTEM_MODE	@ replace the user mode by the SYSTEM mode

no_system_mode:


	mov r0, #save_R13
	bl load_from_host	@ r13 is fetched in r0
	mov r1, r0		@ then in R1

	mov r0, #save_R14
	bl load_from_host	@ r14 is fetched in r0

	nop
	msr cpsr_c, r5	@ switch to the mode of the debugged program
	nop

	mov r13, r1
	mov r14, r0

	@ switch back to debug mode
	nop
	msr cpsr_c, r2
	nop

	short_restore_reg r1, save_R1
	short_restore_reg r2, save_R2
	short_restore_reg r3, save_R3
	short_restore_reg r4, save_R4
	short_restore_reg r5, save_R5
	short_restore_reg r6, save_R6
	short_restore_reg r7, save_R7
	short_restore_reg r8, save_R8
	short_restore_reg r9, save_R9
	short_restore_reg r10, save_R10
	short_restore_reg r11, save_R11
	short_restore_reg r12, save_R12

	restore_reg r0, save_R0
	restore_reg lr, save_LR

	@exits the debug exception
	subs pc, lr, #0

@ Download to memory at high speed
@
@ In :	  r1, memory where to store data
@ Out :   -
@ Mod :   r0, r2

ReceiveFromHost:
	mov r6, lr
	
	@ read address
	bl readrx
	mov r1, r0	@ r1 contains the address

	@ is the transfer 32 or 16 bits ?
	bl readrx
	movS r5, r0	@ r5 is non-null for 16 bits transfer
	movne r4, #2
	moveq r4, #4


	mov r2, #0	@ r2 the sum of incoming data, to check consistency

.hsDownloadLoop:
	@GetWordFromHost
	readrx_macro r0
	@ read TXRXCTRL into the CCs
	mrc p14, 0, r15, c14, c0, 0
	bcc .hsDownloadDone
	beq .hsOverflowed
	cmp r5, #0
	streq r0, [r1], #4	@ store to memory
	strneh r0, [r1], #2	@ store to memory
	add r2, r2, r0		@ sum the data. This steps ensure the success
				@ of the memory access.
	
	b .hsDownloadLoop

.hsOverflowed:
	mov r0, #'!'
	SendWordToHost
	b .hsDownloadLoop

.hsDownloadDone:
	mov r0, r2
	SendWordToHost
	mov pc, r6
	
@ Transmit an array of word to jtag
@ address and size are given through jtag
SendToHost:
	mov r6, lr
	@ read address
	bl readrx
	mov r1, r0
	@ read size
	bl readrx
	mov r2, r0, lsl #2
	@ do the put loop
	mov r3, #0
.SendToHostLoop:
	@ write data 
	ldr r0, [r1, r3]
	nop
	bl writetx
	add r3, r3, #4
	cmp r3, r2 
	bne .SendToHostLoop
	mov r0, #0
	mov pc, r6

SetBreakpoint:
	mov r6, lr

	bl readrx	@ which register of the coprocessor 15 ?
	mov r1, r0	@ answer in r1

	bl readrx	@ value to load in the breakpoint register (IBCRx)
	
	adr r2, coproc_15_table
	add pc, r2, r1,  asl #3	@ jump to the correct mcr
	
coproc_15_table:
	mcr p15, 0, r0, c14, c8, 0	@ IBCR0
	mov pc, r6
	mcr p15, 0, r0, c14, c9, 0	@ IBCR1
	mov pc, r6
	mcr p15, 0, r0, c14, c0, 0	@ DBR0
	mov pc, r6
	mcr p15, 0, r0, c14, c3, 0	@ DBR1
	mov pc, r6
	mcr p15, 0, r0, c14, c4, 0	@ DBCON
	mov pc, r6