trap10.c

mips架构的bootloader,99左右的版本 但源代码现在没人更新了

/*************************************************************
 * File: lib/trap10.c
 * Purpose: Part of C runtime library
 * Author: Phil Bunce ([e-mail])
 * Revision History:
 *	970304	Start of revision history
 *	970928	The unused tab3 commented out.
 */

#include <mips.h>
#include <utypes.h>

#if 0 /* test mode */
#define OP_MULT		024
#define OP_MULTU	025
#define OP_DIV		026
#define OP_DIVU		027
#else
#define OP_MULT		030
#define OP_MULTU	031
#define OP_DIV		032
#define OP_DIVU		033
#endif
#define OP_MFHI		020
#define OP_MFLO		022
#define OP_MTHI		021
#define OP_MTLO		023

/*#define CALL_FUNC /* use func call when writing to instr mem */
/*#define DEBUG /* */

#include "trap10.h"

#ifdef DEBUG
#  define MSG(x)	printf("%s, ",x)
#else
#  define MSG(x)
#endif

#define USE_MACROS

#ifdef USE_MACROS
# define getfield(w,s,p) (((w)&(((1<<(s))-1)<<(p)))>>(p))
#else
Ulong getfield();
#endif

#define BRKINSTR 0x46000010

Trap10state trap10data;
Trap10state *trap10state = &trap10data;

Ulong *dobranch(),*_trap10(),*jmp_targ(),*bra_targ(),*muldiv();
Ulong signextend();

static int bltz(),bgez(),beq(), bne(), blez(),bgtz(),unimp();
static Func *bcc_table[] = {unimp,bltz,bgez,unimp,beq, bne, blez,bgtz};

#if 0 /* not used */
 static int tab3[4][16] = {
/*	   f  un  eq  ueq olt ult ole ule sf ngle seq ngl lt  nge le  ngt */
/* < */  { 0,  0,  0,  0,  1,  1,  1,  1,  0,  0,  0,  0,  1,  1,  1,  1},
/* > */  { 0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0,  0},
/* = */  { 0,  0,  1,  1,  0,  0,  1,  1,  0,  0,  1,  1,  0,  0,  1,  1},
/* u */  { 0,  1,  0,  1,  0,  1,  0,  1,  0,  1,  0,  1,  0,  1,  0,  1}
	};
#endif

/*************************************************************
*  Ulong *_trap10(cause,epc)
*/
Ulong *_trap10(cause,epc)
Ulong cause,*epc;
{
Ulong *pc;

#ifdef DEBUG
printf("cause=%08x epc=%08x instr=%08x\n",cause,epc,*epc);
#endif

if (getfield(cause,1,31) == 0) { 
	MSG("instr is not in delay slot");
	pc = muldiv(epc);
	}
else { 
	MSG("instr is in delay slot");
	pc = dobranch(epc);
	muldiv(epc+1);
	}
return(pc);
}

/*************************************************************
*  Ulong *dobranch(pc)
*/
Ulong *dobranch(pc)
Ulong *pc;
{
if (conditional(pc)) {
	MSG("is conditional");
	if (branchtaken(pc)) {
		MSG("branch taken");
		pc = bra_targ(pc);
		}
	else pc = pc + 2;
	}
else pc = jmp_targ(pc);
return(pc);
}

/*************************************************************
*  conditional(pc)
*/
conditional(pc)
Ulong *pc;
{
Ulong instr;
int x;

instr = *pc;
x = getfield(instr,6,26);
if (x == 0 || x == 2 || x == 3) return(0);
else return(1);
}

/*************************************************************
*  Ulong *bra_targ(pc)
*/
Ulong *bra_targ(pc)
Ulong *pc;
{
Ulong instr,offset;

instr = *pc;
offset = getfield(instr,16,0);
offset = signextend(offset,16);
pc = pc + offset + 1;
return(pc);
}

/*************************************************************
*  Ulong *jmp_targ(pc)
*/
Ulong *jmp_targ(pc)
Ulong *pc;
{
Ulong instr,seg,offset;
int op;

instr = *pc;
op = getfield(instr,3,26); 
if (op == 0) { 
	MSG("jr");
	if (getfield(instr,6,0) == 9) {
		MSG("al");
		trap10state->Gpr[31] = (Ulong) (pc+2);
		}
	pc = (Ulong *)(trap10state->Gpr[getfield(instr,5,21)]);
	}
else { 
	MSG("j");
	if (op == 3) {
		MSG("al");
		trap10state->Gpr[31] = (Ulong) (pc+2);
		}
	seg = getfield((Ulong)pc,4,28);
	offset = getfield(instr,26,0)<<2;
	pc = (Ulong *)((seg<<28) | offset);
	}
return(pc);
}

/*************************************************************
*  branchtaken(pc)
*/
branchtaken(pc)
Ulong *pc;
{
Ulong instr;
int v,rs,rt,x;
Func *func;

instr = *pc;
rs = getfield(instr,5,21);
rt = getfield(instr,5,16);
MSG("bcc");
if (getfield(instr,3,26) == 1 && getfield(instr,1,16) == 1) v = 2;
else v = getfield(instr,3,26);
func = bcc_table[v];
x = (*func)(trap10state->Gpr[rs],trap10state->Gpr[rt]);
return(x);
}

/*************************************************************
*  Ulong *muldiv(pc)
*/
Ulong *muldiv(pc)
Ulong *pc;
{
Ulong instr;
int op,rs,rt,rd;
Func *func;
Ulong hi,lo;
long x,y,rem,quot;
int type,flag,xflag;

instr = *pc;

op = getfield(instr,6,26);
/* if not SPECIAL; then unsupported */
if (op != 0) return (Ulong*)(trap10state->Gpr[S_RTN]); 
rs = getfield(instr,5,21);
rt = getfield(instr,5,16);
type = getfield(instr,6,0);

switch (type) {
    case OP_MFHI: /* rd */
	rd = getfield(instr,5,11);
	trap10state->Gpr[rd] = trap10state->Gpr[S_HI];
	break;
    case OP_MFLO: /* rd */
	rd = getfield(instr,5,11);
	trap10state->Gpr[rd] = trap10state->Gpr[S_LO];
	break;
    case OP_MTLO: /* rs */
	trap10state->Gpr[S_LO] = trap10state->Gpr[rs];
	break;
    case OP_MTHI: /* rs */
	trap10state->Gpr[S_HI] = trap10state->Gpr[rs];
	break;
    case OP_MULT: 
	mul32s(trap10state->Gpr[rs],trap10state->Gpr[rt],&hi,&lo);
	trap10state->Gpr[S_HI] = hi;
	trap10state->Gpr[S_LO] = lo;
	break;
    case OP_MULTU:
	mul32u(trap10state->Gpr[rs],trap10state->Gpr[rt],&hi,&lo);
	trap10state->Gpr[S_HI] = hi;
	trap10state->Gpr[S_LO] = lo;
	break;
    case OP_DIV:
	x = trap10state->Gpr[rs];
	y = trap10state->Gpr[rt];
	flag = 0;
	xflag = 0;
	if (x < 0) { /* if x is -ve, make it +ve */
		flag = 1;
		xflag = 1;
		x = 0-x;
		}
	if (y < 0) { /* if y is -ve, make it +ve */
		flag = (flag)?0:1; /* toggle flag */
		y = 0-y;
		}
	div32u(x,y,&rem,&quot);
	if (flag) { /* fix up the answer */
		quot = 0-quot;
		}
        if (xflag) { /* fix up the answer */
		rem = 0-rem;
		}
	trap10state->Gpr[S_HI] = rem;
	trap10state->Gpr[S_LO] = quot;
	break;
    case OP_DIVU:
	div32u(trap10state->Gpr[rs],trap10state->Gpr[rt],&hi,&lo);
	trap10state->Gpr[S_HI] = hi;
	trap10state->Gpr[S_LO] = lo;
	break;
    default:
	return (Ulong *)(trap10state->Gpr[S_RTN]); /* unsupported: */
    }

if (pc == 0) return((Ulong *)(K1BASE|(unsigned long)(trap10state->ibuf)));
return(pc+1);
}

static bltz(x) {MSG("bltz");return((x<0)?1:0);}
static bgez(x) {MSG("bgez");return((x>=0)?1:0);}
static beq(x,y) {MSG("beq");return((x==y)?1:0);}
static bne(x,y) {MSG("bne");return((x!=y)?1:0);}
static blez(x) {MSG("blez");return((x<=0)?1:0);}
static bgtz(x) {MSG("bgtz");return((x>0)?1:0);}

static unimp() {MSG("unimp");}

#ifndef USE_MACROS
Ulong getfield(src,size,lsb)
Ulong src;
int size,lsb;
{
Ulong msk;

msk = ((1<<size)-1)<<lsb;
return((src&msk)>>lsb);
}
#endif

/*************************************************************
*  Ulong signextend(v,n)
*/
Ulong signextend(v,n)
Ulong v;
int n;
{
long r;

v <<= (32-n);
r = v;
r >>= (32-n);
return((Ulong)r);
}

/*************************************************************
*  mul32s(src1,src2,hi,lo)
*	32-bit signed multiply
*/
mul32s(src1,src2,hi,lo)
long src1,src2;
long *hi,*lo;
{
long q,dest;
int i;

q = src2;
if (q&1) dest = src1;
else dest = 0;
srad(&dest,&q);
for (i=0;i<30;i++) {
	if (q&1) dest += src1;
	srad(&dest,&q);
	}
if (q&1) dest -= src1;
srad(&dest,&q);
*hi = dest;
*lo = q;
}

/*************************************************************
*  mul32u(src1,src2,hi,lo)
*	32-bit unsigned multiply
*/
mul32u(src1,src2,hi,lo)
Ulong src1,src2;
Ulong *hi,*lo;
{
Ulong q,dest,t;
int i,cf;

q = src2;
if (q&1) dest = src1;
else dest = 0;
cf = 0;
srldc(cf,&dest,&q);
for (i=0;i<31;i++) {
	t = dest;
	if (q&1) {
		t = dest + src1;
		if (t < src1 || t < dest) cf = 1;
		else cf = 0;
		}
	dest = t;
	srldc(cf,&dest,&q);
	}
*hi = dest;
*lo = q;
}

/*************************************************************
*  srad(p1,p2)
*	shift right arithmetic double
*	p1 -> p2
*/
srad(p1,p2)
long *p1;
Ulong *p2;
{
(*p2) >>= 1;
(*p2) |= (*p1)<<31;
(*p1) >>= 1;
}

/*************************************************************
*  srldc(c,p1,p2)
*	shift right logical double with carry
*	c -> p1 -> p2
*/
srldc(c,p1,p2)
int c;
Ulong *p1,*p2;
{
(*p2) >>= 1;
(*p2) |= (*p1)<<31;
(*p1) >>= 1;
(*p1) |= c<<31;
}

/*************************************************************
*  diffsign(a,b)
*	return 1 if the signs are different
*/
diffsign(a,b)
Ulong a,b;
{
if ((a&0x80000000) != (b&0x80000000)) return(1);
return(0);
}