/*--------------------------------------------------*/
        /*	XMD_stub version 1.0                        */
	/*      MicroBlaze                                  */
	/*      (c) Xilinx 2001                             */
	/*      Author: G鰎an Bilski                        */
	/*      Created:  11 Juli 2001	                    */
	/*      Modified: 20 Sept 2001                      */	
	/*	Modified: 08 Nov  2001                      */
	/*	Modified: 12 Mar  2002 (jece & navanee)     */	
	/*--------------------------------------------------*/
	.globl  _start	
	.text
	.align	2
	.globl	xmd_stub
	.ent	xmd_stub
_start:
xmd_stub:
	.equ uart_rx_data,         DEBUG_PERIPHERAL_BASEADDRESS
	.equ uart_tx_data,         DEBUG_PERIPHERAL_BASEADDRESS +  4
	.equ uart_stat,	           DEBUG_PERIPHERAL_BASEADDRESS +  8
	.equ uart_ctrl,		   DEBUG_PERIPHERAL_BASEADDRESS + 12
	
	.equ uart_tx_buffer_empty, 0b100
	.equ uart_rx_data_valid,   0b001
	
	.equ uart_rst_rx_fifo,      0b010
	.equ uart_rst_tx_fifo,      0b001
	
	.equ Reached_End_Of_Program, 0x00
	.equ Reached_Breakpoint,     0x01
	.equ Received_brk_signal,    0x02
	.equ ProcReset_signal,       0x03
	.equ Escape_Char,            0xFC
	.equ ReSync_Char,            0xFE
	.equ Stub_ID,                0x40

	.equ Reset_TxFifo,	     0x01
	.equ Reset_RxFifo,	     0x02
	.equ Clear_ExtBreak,	     0x04
	.equ Intr_Enabled,	     0x10
	
	.equ version, 2

	/*--------------------------------------------------*/
	/* MicroBlaze jump table                            */
        /* 0x0  - Reset address                             */	
	/* 0x8  - Exception address                         */
        /* 0x10 - Interrupt address                         */
        /* 0x18 - Break signal address (short branch)       */
	/* 0x1c - Breakpoint address (short branch)         */	
        /* 0x20 - Program Exit(crt1.s) (short branch)       */
	/*--------------------------------------------------*/
		
	brki r16, StartUp_Entry
	nop
	nop
	nop	
	nop
	nop
	bri Brk_Signal_Entry
	bri Debug_Entry
	bri Program_Exit

	/*--------------------------------------------------*/
	/* ReSync                                           */
	/* - Remove any stale data from the RX and TX fifo  */
	/* - Send back the stubID + version number          */
	/*--------------------------------------------------*/
ReSync:
	lwi  r15, r0, uart_stat
	andi r15,r15, Intr_Enabled
	ori  r15, r15, uart_rst_tx_fifo + uart_rst_rx_fifo
	swi  r15, r0, uart_ctrl
	brlid r29,Wait_for_TX_space
	addi r30,r0,Stub_ID + version
	swi  r30,r0,uart_tx_data

	/*--------------------------------------------------*/
	/* Main command entry code                          */
	/* - Wait for the next command                      */
	/*    0 = Download Program                          */
	/*    4 = Continue                                  */
	/*    8 = Read Registers                            */
	/*   12 = Write Register                            */
	/*   16 = Read Memory                               */
	/*   20 = Write Memory                              */
	/*   24 = Resync                                    */
	/*   28 = SingleStep                                */
	/* - Call the command without any link              */
	/*--------------------------------------------------*/
Wait_for_command:
	brlid r29,Get_Char
	nop
	lwi r29,r3,jump_table
	bra r29

	/*--------------------------------------------------*/
	/* This section will end up in the rodata section    */
	/* Is place here for helping understanding the code */
	/*--------------------------------------------------*/
	.rodata
	.align 2
jump_table:
  	.int Load_Memory
  	.int Execute
  	.int Read_Register 
  	.int Write_Register
	.int Read_Memory
  	.int Write_Memory 
	.int ReSync
	.int Return_Dbg_Addr
	.int Continue
	.int SingleStep


	/*--------------------------------------------------*/
	/* Go back to the text section(instruction)         */
	/*--------------------------------------------------*/	
	.text


	/*--------------------------------------------------*/	
	/* Read all registers command                       */
	/* Get the values of all registers                  */
	/*   from the register_dump where the register      */
	/*   values are stored                              */
	/* - Send all values to the terminal (through UART) */
	/*   33 words will be sent: 1-31 and pc and msr     */
	/*   Note that the pc value sent is always incorrect*/
	/*   It has to be in the table for consistency      */
	/* - Jump to the main command entry code            */
	/*--------------------------------------------------*/	
Read_Register:	
	addi r28, r0, register_dump+4
	addi r26, r0, 33
Read_Register_1:		
	brlid r15, Send_32Bit
	lw   r5, r28, r0
	addi r26,r26,-1
	bneid r26, Read_Register_1
	addi r28,r28,4
	bri Wait_for_command
	
	/*--------------------------------------------------*/	
	/* Write to a register value command                */
	/* - Get a byte which is the register asked for     */
	/*   The byte has to be Register_Number*4,          */
	/*   since the value will be used as word address   */
        /*   and the addressing is always in byte addresses */
	/*   ex. Want to see value of register 15           */
	/*       The byte value has to be 4*15 = 60         */
	/* - Get 32 bits (4 bytes) which is the new value   */
	/*   for the register                               */
	/* - Store the new value to the stored value for    */
	/*   that register from the register_dump where the */
	/*   register values are stored                     */
	/* - Jump to the main command entry code            */
	/*--------------------------------------------------*/	
Write_Register:	
	/* Write a register value                    */
	/* Next byte is which register               */
	/* Next four bytes is the new register value */
	brlid r29,Get_Char
	nop
	brlid r15,Get_32Bit
	or r28,r3,r0
	swi r3,r28,register_dump
	bri Wait_for_command


	/*--------------------------------------------------*/	
	/* Read from memory command                         */
	/* - Get 32 bits (4 bytes) which is the memory      */
	/*   address.                                       */
	/* - Read the value from that memory address        */
	/*   for the register                               */
	/* - Send that value to the terminal (through UART) */
	/* - Jump to the main command entry code            */
	/*--------------------------------------------------*/	
Read_Memory:
	/* Next four bytes is the address */
	/* The response will be the value in that address */
	/* send as four bytes */
	brlid r15,Get_32Bit
	nop
	brlid r15,Send_32Bit
	lw r5,r3,r0
	bri Wait_for_command


	/*--------------------------------------------------*/	
	/* Write to memory command                          */
	/* - Get 32 bits (4 bytes) which is the memory      */
	/*   address.                                       */
	/* - Get 32 bits (4 bytes) which is the new value   */
	/*   for that memory address.                       */
	/* - Write the new value to the memory.             */
	/* - Jump to the main command entry code            */
	/*--------------------------------------------------*/	
Write_Memory:
	/* Next four byte is the address */
	/* Then the next four byte is the new data */
	brlid r15,Get_32Bit
	nop
	brlid r15,Get_32Bit
	or r28,r3,r0
	swi r3,r28,0
	bri Wait_for_command


	/*--------------------------------------------------*/	
	/* Download program command                         */
	/* - Get 32 bits (4 bytes) which is the start       */
	/*   address for the downloading                    */
	/* - Get 32 bits (4 bytes) which is the number of   */
	/*   bytes in the downloading                       */
	/* - Loop                                           */
	/*   - Get a byte                                   */
	/*   - Store it at the memory address               */
	/*   - Increment the memory address with 1          */
	/*   - Decrement the number of bytes by 1           */
	/*   - Continue until all bytes has been received   */
	/* - Jump to the main command entry code            */
	/*--------------------------------------------------*/	
Load_Memory:	
	/* Next 4 bytes is the start address */
	/* The next four bytes is the number of bytes */
	brlid r15,Get_32Bit
	nop
	brlid r15,Get_32Bit
	or r28,r3,r0
	or r25,r3,r0
Load_Memory_1:	
	brlid r29,Get_Char
	addi r25,r25,-1
	sb r3,r28,r0
	bneid r25,Load_Memory_1
	addi r28,r28,1
	bri Wait_for_command				



	/*--------------------------------------------------*/
	/* NOTE : EXECUTE FOR THE SAKE OF COMPATIBILITY WITH MDK 2.2 */
	/* Execute program command                          */
	/* - Get 32 bits (4 bytes) which is the memory      */
	/*   address.                                       */
	/* - Load all register from the register_dump       */
	/*--------------------------------------------------*/	
Execute:
	/* Next four byte is the start address */
	brlid r15,Get_32Bit
	nop
	/* Set the R16 value in the reg dump to the start addr  */
	/* When control falls thru to the Continue command      */
	/* it will start execution from the addr in the new R16 */
	swi r3,r0,register_dump+64
	/* Fall thru to the following Continue command */

	
	/*--------------------------------------------------*/
	/* Continue command                                 */
	/* continue at the instruction after the brk        */
	/*  - Restore all registers                         */
	/*  - jump to the address for the BRK instruction   */
	/*--------------------------------------------------*/
Continue:
	brlid r15, Get_All_Registers
	nop
	rtbd r16, 0
	lwi r15,r0,register_dump+15*4


	/*--------------------------------------------------*/
	/* SingleStep command                               */
	/* step single instruction without enabling         */
	/* the BIP flag                                     */
	/*  - Restore all registers                         */
	/*  - jump to the address for the BRK instruction   */
	/*--------------------------------------------------*/
SingleStep:
	brlid r15, Get_All_Registers
	nop
	rtsd r16, 0
	lwi r15,r0,register_dump+15*4

	/*--------------------------------------------------*/	
	/* When Processor resets                            */
	/*   - save all registers to the register_dump      */
	/*   - send 0x3 ('Processor reset') to host         */
	/*   - Jump to the main command entry code          */
	/*--------------------------------------------------*/	
	
StartUp_Entry:
	swi r15,r0,register_dump+60
	brid Dump_All_Registers
	ori r15,r0,ProcReset_signal
	/* Control goes to SendStatus from DumpAllRegisters */
	
	
	/*--------------------------------------------------*/	
	/* Program End entry                                */