/*====================================================================
 *
 * File:        gdbStep.c
 * Author:      Steve Shepard (sjs@sgi.com)
 *
 *====================================================================*/

#include <ultratypes.h>
#include <os_internal.h>
#include <sys/inst.h>
#include "ultragdb.h"

typedef u32 inst_t;
typedef u32 __psunsigned_t;

typedef struct {
    inst_t      *addr;
    u32         data;
} TmpBreak;

TmpBreak    breakList[2];
int         breakCnt;

/*
 * branch_target - return address where branch will go
 */
#define	REGVAL(x)	( ((u32 *)&reg) [(x)] )

int
is_branch(inst_t inst)
{
	union mips_instruction i;

	i.word = inst;
	switch (i.j_format.opcode) {
	case spec_op:
		switch (i.r_format.func) {
		case jr_op:
		case jalr_op:
			return(1);
		}
		return(0);

	case bcond_op:
		switch (i.i_format.rt) {
		case bltz_op:
		case bgez_op:
		case bltzal_op:
		case bgezal_op:
		case bltzl_op:
		case bgezl_op:
		case bltzall_op:
		case bgezall_op:
			return(1);
		}
		return(0);

	case j_op:
	case jal_op:
	case beq_op:
	case bne_op:
	case blez_op:
	case bgtz_op:
	case beql_op:
	case bnel_op:
	case blezl_op:
	case bgtzl_op:
		return(1);

	case cop0_op:
	case cop1_op:
	case cop2_op:
	case cop3_op:
		switch (i.r_format.rs) {
		case bc_op:
			return(1);
		}
		return(0);
	}
	return(0);
}

static inst_t *
branch_target(inst_t inst, inst_t *pc)
{
    union mips_instruction i;
    register short simmediate;

    i.word = inst;
    switch (i.j_format.opcode) {
      case spec_op:
          switch (i.r_format.func) {
            case jr_op:
            case jalr_op:
                return ((inst_t *)(REGVAL(i.r_format.rs)));
          }
          break;

      case bcond_op:
          switch (i.i_format.rt) {
            case bltz_op:
            case bgez_op:
            case bltzal_op:
            case bgezal_op:
            case bltzl_op:
            case bgezl_op:
            case bltzall_op:
            case bgezall_op:
                /*
                 * assign to temp since compiler currently
                 * doesn't handle signed bit fields
                 */
                simmediate = i.i_format.simmediate;
                return ((inst_t *)((__psunsigned_t)pc+4+(simmediate<<2)));
          }
          break;

      case j_op:
      case jal_op:
          return((inst_t *)((((__psunsigned_t)pc+4)&~((1<<28)-1)) | (i.j_format.target<<2)));

      case beq_op:
      case bne_op:
      case blez_op:
      case bgtz_op:
      case beql_op:
      case bnel_op:
      case blezl_op:
      case bgtzl_op:
          /*
           * assign to temp since compiler currently
           * doesn't handle signed bit fields
           */
          simmediate = i.i_format.simmediate;
          return ((inst_t *)((__psunsigned_t)pc+4+(simmediate<<2)));

      case cop0_op:
      case cop1_op:
      case cop2_op:
      case cop3_op:
          switch (i.r_format.rs) {
            case bc_op:
                /*
                 * kludge around compiler deficiency
                 */
                simmediate = i.i_format.simmediate;
                return((inst_t *)((__psunsigned_t)pc+4+(simmediate<<2)));
          }
          break;
    }

    if (gdbDebug)
        osSyncPrintf("illegal instruction in branch_target\n");

    return pc;
}

void set_bp(inst_t *loc, int i)
{
    if (gdbDebug)
        osSyncPrintf("setting tmp bp %d at 0x%x\n", i, loc);
    
    breakList[i].addr = loc;
    breakList[i].data = *loc;

    *loc = TMP_BREAK;          /* break instruction */

    gdbWritebackDCache((void *)loc, 4);
    gdbInvalICache((void *)loc, 4);

}

void installBP(void) 
{
    inst_t inst;
    inst_t *target;
    inst_t *pc;
    
    /* ### what if this triggers a watch point??? */

    breakCnt = 0;
    
    pc = (inst_t *)reg.pc;
    inst = *pc;     /* ### should check to make sure this is valid!!!*/
    
    if (is_branch(inst)) {
        target = branch_target(inst, pc);

        /*
         * Can't single step self-branches, so just wait
         * until they fall through
         */
        if (target != pc)
            set_bp(target, breakCnt++);         /* set branch target */
        set_bp((inst_t *)pc+2, breakCnt++);     /* and branch not taken loc */
    } else
        set_bp((inst_t *)pc+1, breakCnt++);     /* bp one instruction later */
}

s32 removeBP(void) 
{
    int i;

    /* need to remove in reverse order */
    for (i = --breakCnt; i >= 0; i--) {
        if (gdbDebug)
            osSyncPrintf("removing tmp bp %d at 0x%x\n", i, breakList[i].addr);
        
        *breakList[i].addr = breakList[i].data;
        gdbWritebackDCache((void *)breakList[i].addr, 4);
        gdbInvalICache((void *)breakList[i].addr, 4);
    }
    
    breakCnt = 0;

    return 1;
}