/*----------------------------------------------------------------------
 *
 * Copyright (c) 2005 Xilinx, Inc.  All rights reserved. 
 * Xilinx, Inc. 
 *
 * XILINX IS PROVIDING THIS DESIGN, CODE, OR INFORMATION "AS IS" AS A 
 * COURTESY TO YOU.  BY PROVIDING THIS DESIGN, CODE, OR INFORMATION AS 
 * ONE POSSIBLE   IMPLEMENTATION OF THIS FEATURE, APPLICATION OR 
 * STANDARD, XILINX IS MAKING NO REPRESENTATION THAT THIS IMPLEMENTATION 
 * IS FREE FROM ANY CLAIMS OF INFRINGEMENT, AND YOU ARE RESPONSIBLE 
 * FOR OBTAINING ANY RIGHTS YOU MAY REQUIRE FOR YOUR IMPLEMENTATION. 
 * XILINX EXPRESSLY DISCLAIMS ANY WARRANTY WHATSOEVER WITH RESPECT TO 
 * THE ADEQUACY OF THE IMPLEMENTATION, INCLUDING BUT NOT LIMITED TO 
 * ANY WARRANTIES OR REPRESENTATIONS THAT THIS IMPLEMENTATION IS FREE 
 * FROM CLAIMS OF INFRINGEMENT, IMPLIED WARRANTIES OF MERCHANTABILITY 
 * AND FITNESS FOR A PARTICULAR PURPOSE. 
 *
 * $Header: $
 * $Id: $
 *
 *	XMD_stub version 1.0                        
 *      MicroBlaze                                  
 *      Author: G鰎an Bilski                        
 *      Created:  11 Juli 2001	                   
 *      Modified: 20 Sept 2001                      	
 *	Modified: 08 Nov  2001                     
 *	Modified: 12 Mar  2002 (jece & navanee)    	
 *	Modified: 25 Aug  2004 (navanee)
 *	Modified: Jan 2005 (Raj - Added Half Word/Byte Read & Write)	 
 *
 ----------------------------------------------------------------------*/

	.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, 3

	/*--------------------------------------------------*/
	/* MicroBlaze jump table                            */
        /* 0x0  - Reset address                             */	
	/* 0x8  - Exception address                         */
        /* 0x10 - Interrupt address                         */
        /* 0x18 - Break signal address (short branch)       */
	/* 0x1c - Breakpoint address (short branch)         */	
	/* 0x20 - Hardware Exception addr (MB ref guide)    */
        /* 0x28 - Program Exit(crt1.s) (short branch)       */
	/*--------------------------------------------------*/
		
	brki r16, StartUp_Entry
	nop
	nop
	nop	
	nop
	nop
	bri Brk_Signal_Entry
	bri Debug_Entry
	nop
	nop
	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 = Return Debug Addr - Deprecated	    */
	/*   32 = Continue				    */	
	/*   36 = SingleStep                                */
	/*   40 = Read Memory Half Word			    */
	/*   44 = Read Memory Byte			    */
	/*   48 = Write Memory Half Word		    */
	/*   52 = Write Memory Byte			    */
	/* - 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
	.int Read_Memory_Half
	.int Read_Memory_Byte
	.int Write_Memory_Half
	.int Write_Memory_Byte
	
	/*--------------------------------------------------*/
	/* 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

	/*--------------------------------------------------*/	
	/* Read from memory Half Word 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_Half:
	/* Next four bytes is the address */
	/* The response will be the value in that address */
	/* send as two bytes */
	brlid r15,Get_32Bit
	nop
	brlid r15,Send_16Bit
	lhu r5,r3,r0
	bri Wait_for_command

	/*--------------------------------------------------*/	
	/* Read from memory Byte 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_Byte:
	/* Next four bytes is the address */
	/* The response will be the value in that address */
	/* send a byte */
	brlid r15,Get_32Bit
	nop
	brid Send_Status
	lbu r15,r3,r0

	/*--------------------------------------------------*/	
	/* 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

Write_Memory_Half:
	/* Next four bytes is the address */
	/* Then the next two bytes is the new data */
	brlid r15,Get_32Bit
	nop
	brlid r15,Get_16Bit
	or r28,r3,r0
	shi r3,r28,0
	bri Wait_for_command

Write_Memory_Byte:
	/* Next four byte is the address */
	/* Then the next byte is the new data */
	brlid r15,Get_32Bit
	nop
	brlid r29,Get_Char
	or r28,r3,r0
	sbi 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   */
	/*--------------------------------------------------*/