/* SPDX-License-Identifier: GPL-2.0-or-later */

/***************************************************************************
 *   Copyright (C) 2006 by Dominic Rath                                    *
 *   Dominic.Rath@gmx.de                                                   *
 ***************************************************************************/
#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