/*---------------------------------------------------------------------*
 *  Copyright (C) 2004 BroadOn Communications Corp.
 *                 
 *  $RCSfile: gdbregs.c,v $ (corresponds to rmonregs.c)
 *  $Revision: 1.1 $
 *  $Date: 2004/02/06 02:12:22 $
 *---------------------------------------------------------------------*/

/* 
 * This file contains functions for setting/accessing registers for the
 * GDB stub.
 */

#include "ultragdb.h"

/**************** GDB Register Access Functions **************/

static Registers reg;

/************************************************************************
Function: __gdbGetRegisterContents
Args: OSThread* tptr - which thread to use for reg info
	  int regNumber - which register to return
Type: u32 - returns the contents of the register
Purpose: This function obtains the contents of the register specifed from
	either the static application register storage area or the Thread
	Control Block. This function is called by the routine that sets
	breakpoints for stepping through a jump to register contents inst.
************************************************************************/
u32 __gdbGetRegisterContents( OSThread* tptr, int regNumber )
{
    u32 * regPointer;

    if (tptr == NULL) return 0;
    
    if ( regNumber >= 1 && regNumber <= 25 )
        regNumber -= 1;	/* no stored 0 */
    else if ( regNumber >= 28 && regNumber <= 31 )
        regNumber -= 3;	/* no stored 0, 26, 27 */
    else
        return 0;	/* not available */
    
    regPointer = (u32 *) &tptr->context;
    regPointer += regNumber;
    return *regPointer;
}

/************************************************************************
 Function: __gdbSetRegister
 Args: 	   OSThread* thread - Thread for which the register should be set
           int reg   - Register number.  The register order follows
                       that expected by GDB for mips-linux.
                       (see ultragdb.h and GDB's tm-mips.h)
`          int value - Value to set the register to
 Purpose:  This function sets the specified register to value for the
           given thread.
 Returns:  int - Error code (<0 indicates error, >= 0 is OK)
           GDB_ERROR_NO_ERROR   (0) - Register successfully set
           GDB_ERROR_INVALID_ID     - Invalid register or thread
************************************************************************/
int __gdbSetRegister(OSThread* thread, int reg, int value)
{
    volatile float f;
    __OSThreadContext	*c;
    
    /*
     * Touch a floating point register to become the owner of the floating
     * point unit (and force the previous owner to save its registers.
     */
    f = 0.0F;

    if (thread == NULL)
    {
        /* Argh, we can't find any threads */
        return GDB_ERROR_INVALID_ID;
    }

    c = &thread->context;

    if (reg == 0)
    {
        /* Cannot set this register - R0 tied to 0 */
        return GDB_ERROR_INVALID_ID;
    }
    else if ((reg >= 1) && (reg <= 25))
    {
        /* General purpose registers - u64 in thread context */
        u64 *regptr = &c->at;
        regptr[reg-1] = value;
    }
    else if ((reg == 26) || (reg == 27))
    {
        /* Cannot set these registers - K0 and K1 are not in the thread context */
        return GDB_ERROR_INVALID_ID;
    }
    else if ((reg >= 28) && (reg <= 31))
    {
        /* General purpose registers - u64 in thread context */
        u64 *regptr = &c->gp;
        regptr[reg-28] = value;
    }
    else if (reg == 32)
    {
        c->sr = value;
    }
    else if (reg == 33)
    {
        c->hi = value;
    }
    else if (reg == 34)
    {
        c->lo = value;
    }
    else if (reg == 35)
    {
        c->badvaddr = value;
    }
    else if (reg == 36)
    {
        c->cause = value;
    }
    else if (reg == 37)
    {    
        c->pc = value;
    }
    else if ((reg >= 38) && (reg <= 69))
    {
        /* Floating point registers */
        u32 *regptr = (u32*) &c->fp0;
        regptr[reg-38] = value;
    }
    else if (reg == 70)
    {
        c->fpcsr = value;
    }
    else 
    {
        return GDB_ERROR_INVALID_ID;        
    }

    return GDB_ERROR_NO_ERROR;
}

/************************************************************************
 Function: gdbProcSetRegister
 Args: 	   char* ptr  - Set Register command (format: n=value)
           char* resp - Response buffer
 Purpose:  This function parses the GDB set register command and sets
           the specified register for the current thread.  This function
           will also write to the response buffer (null-terminated)
           the status to be sent to GDB:
           OK  - If the operation was successful
           ENN - If there was an error
 TODO:     If current thread is -1, set register for all threads????
************************************************************************/
void gdbProcSetRegisterPkt(char* ptr, char *resp)
{
    int reg, value;

    if ((ptr == NULL) || (resp == NULL)) return;

    if (hexToInt(&ptr, &reg)    /* get register number */
        && (*ptr++ == '=')
        && hexToInt(&ptr, &value))
    {
        OSThread* t = getOneCurrentThread();
        if (t != NULL) {
            if (__gdbSetRegister(t, reg, value) >= 0)
                strcpy(resp, GDB_RESP_STATUS_OK);
            else strcpy(resp, GDB_RESP_ERR_INVALID_ADDR);
        }
        else strcpy(resp, GDB_RESP_ERR_NO_THREAD);
    }
    else strcpy(resp, GDB_RESP_ERR_PKT_FORMAT);
}

/************************************************************************
 Function: gdbAssembleRegisters
 Args:     char* resp - Response buffer
 Purpose:  This function responds to the GDB get register request by 
           assembling the registers for the current thread and placing
           it in the response buffer (null-terminated).  The registers
           are given in the order specified by GDB for mips-linux
           (see ultragdb.h and GDB's tm-mips.h) Each register is given 
           in the target-byte order and is 4 bytes wide.

           The response to GDB will be:
           <registers in hex>  - If the operation was successful
           ENN                 - If there was an error
 TODO:     What to do if current thread is -1 (i.e. all threads)?
************************************************************************/
void gdbAssembleRegisters(char *resp)
{
    u32 *src;
    u32 *dest;
    int i;
    OSThread *t;
    __OSThreadContext	*c;
    volatile float f;

    if (resp == NULL) return;
    
    /*
     * Touch a floating point register to become the owner of the floating
     * point unit (and force the previous owner to save its registers.
     */
    f = 0.0F;

    t = getOneCurrentThread();
    if (t == NULL)
    {
        /* Argh, we can't find any threads */
        strcpy(resp, GDB_RESP_ERR_NO_THREAD);
        return;
    }

    c = &t->context;    
    
    /* Copies general registers */
    reg.zero    = 0;
    reg.at      = c->at;
    reg.v0      = c->v0;
    reg.v1      = c->v1;
    reg.a0      = c->a0;
    reg.a1      = c->a1;
    reg.a2      = c->a2;
    reg.a3      = c->a3;

    reg.t0      = c->t0;
    reg.t1      = c->t1;
    reg.t2      = c->t2;
    reg.t3      = c->t3;
    reg.t4      = c->t4;
    reg.t5      = c->t5;
    reg.t6      = c->t6;
    reg.t7      = c->t7;

    reg.s0      = c->s0;
    reg.s1      = c->s1;
    reg.s2      = c->s2;
    reg.s3      = c->s3;
    reg.s4      = c->s4;
    reg.s5      = c->s5;
    reg.s6      = c->s6;
    reg.s7      = c->s7;

    reg.t8      = c->t8;
    reg.t9      = c->t9;
    reg.k0      = 0; /* Not in thread context */
    reg.k1      = 0; /* Not in thread context */
    reg.gp      = c->gp;
    reg.sp      = c->sp;
    reg.s8      = c->s8;
    reg.ra      = c->ra;

    /* Copies special registers for main CPU */
    reg.sr      = c->sr;
    reg.lo      = c->lo;
    reg.hi      = c->hi;
    reg.bad     = c->badvaddr;
    reg.cause   = c->cause;
    reg.pc      = c->pc;

    /* Copies floating point registers */
    dest = (u32 *)&reg.f0;
    src  = (u32 *)&c->fp0;
    
    for (i = 0; i < 32; i++)
        *dest++ = *src++;
    
    /* Copies flating point control registers */
    /* The R4300 is suppose to have 32 control registers - most of which we ignore 
       since it's unclear what they do and GDB does not seem to need them */
    reg.fsr     = c->fpcsr; /* Floating point control/status register? */


    /* Registers are given in target endianess */
    mem2hex((char *)&reg, resp, sizeof(Registers));    
}