/************************************************************
 * File: bsps/iceif.c
 * Purpose: serialIce interface code - modified to support 64-bit CPUs
 * Author: Phil Bunce (pjb@carmel.com)
 * Revision History:
 *	961220	Created
 *	970216	Switched over to addCmdRec for extra commands.
 *	970218	Added dflush_needed logic.
 *	970224	Added dbx logic. works for bda.
 *	970225	Flush not needed if write is to internal register.
 *	970226	Delay flush until just before RUN.
 *	970304	Changed type of ch to Uchar in wakeup() (CYGNUS)
 *	970310	Moved brkInstall and brkRemove from here
 *	970310	Changed verbose cmd to use a level
 *	970310	Rewrote setbp_target()
 *	970311	Created bptReq() to handle bp requests from xvw and gdb
 *	970313	Rationalized the calls to init and wakeup (hopefully).
 *	970401	Added unaligned read_target to support MIPS16 disassembly
 *	970605	Changed is_writeable to use a byte (for 4101)
 *	970817	Added Hi and LO as PC+1 and PC+2
 *	970905	Switched over to new send_buffer
 *	970905	Added A1 case to write_target and read_target
 *	970905	Changed to new version of swap32n
 *	970905	p in send_buffer changed to Uchar
 *	970917	Added perens to self bra test in setTrcbp.
 *	970917	Added _ctrl(22) target_stopped.
 *	971023	Added REV1_WIGGLER switch.
 *	971112	Added more cmds to "portconfig".
 *	980127	Added no_gpr30 cmd. Needed for multi in "debug 16" mode.
 *	980127	Added send_ack_each_word cmd. Removed REV1_WIGGLER switch.
 *	980127	Removed USE_SIM_TARG switch.
 *	980127	Changed printDiag to use logfp. Eliminated logfile2.
 *	980127	Changed logfile to not append.
 *	980128	Added siemens_mbd cmd
 *	980128	Added ocm support
 *	980130	Removed 'verbose'. Now using 'logging'.
 *	980202	Added 'breakcode'.
 *	980204	Renamed bptReq, is_bptReq.
 *	980226	Fixed a bunch of stuff incl SRUN.
 *	980305	Changed siemens_mbd to force_wakeup
 *	980309	Added resync
 *	980309	Changed force_wakeup to force_resync
 *	980309	Added force_getsap
 *	980312	Merged iceif.c for dll and imon
 *	980323	Removed send_buffer from start of send_instr (recursive)
 *	980704	Changed write_target hword to bypass the buffer.
 *	980706	Added support for Siemens mbd kernel (force_sap).
 *	980707	Added support for ocm routines on dll w v1 kernel.
 *	980720	Added nobrkRemove. Needed for force_getsap.
 *	980727	Added resyncOk.
 *	980803	Added mallocOcmRec and freeOcmRec.
 *	980818	Added getSAP before write_target_byte calls.
 *	980821	Added undoc1.
 *	980903	Started adding ejtag support.
 *	981214	Made setbp_target less dbx-specific. Removed dcs member.
 *	981216	Added mask support to ibpts in setbp_target.
 *	981216	Started 64-bit mods.
 *	981222	Fixed problem with corrupted downloads on 4011.
 *	990304	Fixed prob when writing 0000aa00 to memory.
 *	990319	Fixed prob w writing to addr zero.
 *	990401	Added FILL to optimize fill ops with ejtag.
 *	990428	Changed EXE64 to pass &Ulong[2].
 */

#include <malloc.h>
#include "iceif.h"


#define BUFSZ	4096
#define MAXRL	65535
#define RLTHRESHOLD 84	/* rlcnt of > the value use set_bytes() */
#define MAXRETRIES 5    /* max number of attempts to contact target */

char *excodes[] = { /* includes r4000 codes */
	"Int","MOD","TLBL","TLBS","AdEL","AdES","IBE","DBE",
        "Sys", "Bp","RI","CpU","Ov","Tr","Resv","FPE",
	"Resv","Resv", "Resv","Resv","Resv","Resv","Resv", "WATCH",
	"Resv","Resv","Resv","Resv","Resv","Resv", "Resv","Resv",
	0};


char *xtcodes[] = { "PC","MEM","GPR","CP0", "CP1","CP1C",
	"CP2","CP2C", "CP3","CP3C", "DBX",0};

Dstate dstate = clean;

U64 current_r8,current_r9;
int instr_buffer_cnt;
Ulong instr_buffer[300]; /* must be larger than the largest ibs */
int lendian_host;
int lendian_target;
int ejtag_mode;

int has_wb_dcache;		/* cpu has a write-back (copy-back) dcache */
int need_initial_flush = 1; /* default is yes */
int target_stopped;
int iflush_needed;
int dflush_needed;
int ice_baud;
char *ice_device;
int nonrt;
int slow_target;
int dbx_needs_pa;	/* the dbx module needs physical addresses */
int resyncOk;		/* kernel accepts 55 at any time */
int undoc1; 		/* undocumented cmd for test use only */

/* For version 3 kernels, as ver 2 but always returns 8 bytes (64 bits).
 * For version 2 kernels, ibs and regmap comes from the kernel.
 * For version 1 kernels, you must specify it in the config file.
 * However, it is only critical for cases where an ocm routine is provided 
 */
int kernel_ibs;		/* kernel ibs value */
Ulong kernel_map;	/* kernel regmap */

struct {
	int rlcnt;	/* run-length count */
	int bcnt;	/* byte-count */
	Uchar *buf;	/* the buffer */
	Ulong start_addr; /* start address of buffer */
	Ulong expected_addr;
	Uchar prev_v;
	int sz;		/* data size 1,2 or 4 (b, h, or w) */
	} ice;

OcmRec *ocmChain,*freeOcmChain;

BrkList brkList[MAX_BPT];

int has_nhwib;	/* has N h/w instr break regs */
int has_nhwdb;	/* has N h/w data break regs */

int icache_size;
int dcache_size;
int icache_line_size;
int dcache_line_size;
int has_ilock;	/* true or false */
int _cputype;

vFunc *brkInstall_ptr;
Func *ilockReq_ptr;
Func *flush_target_ptr;
Func *brkRemove_ptr;
vFunc *setFlushneeded_ptr;

int no_gpr30;
int send_ack_each_word;
int force_getsap;
Ulong breakcode;
int nobrkRemove;

SaveArea savearea; /* info about the serialice kernel */


/*************************************************************
*  int wakeup(void) DEF
*/
int wakeup(void)
{
printDiag(1,"wakeup() ");
if (!resync()) return(0);
printDiag(1,"wakeup: connection established.\n");
dstate = dirty; /* must assume that cache is dirty after RUN */
brkRemove(0);
return(1);
}

/*************************************************************
*  int resync(void) DEF
*	This routine only works for version 2 or greater kernels.
*/
int resync(void)
{
int i,timeout,ackcnt;
Uchar ch;
static int resync_active;

if (resync_active) return(0); /* avoid unintended recursion */
resync_active = 1;

printDiag(1,"resync() ");
current_r8 = current_r9 = mkRV(0);

#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode) {
	extern char *init_fname;
	if (target_stopped) {
		resync_active = 0;
		return(1);
		}
	printDiag(3,"EjtagControl(WAKEUP)\n");
	if (!EjtagControl(EJTAG_WAKEUP,0,0)) {
		printDiag(1,"resync failure.\n");
		resync_active = 0;
		return(0);
		}
	printDiag(1,"resync achieved.\n");
	target_stopped = 2;
	if (init_fname) {
		sload_init(init_fname);
		init_fname = 0;
		}
	resync_active = 0;
	return(1);
	}
#endif

flushInput();
/* Try a bunch of times. Use a short timeout for the first 4 attempts.
 * Then switch to 1 sec for the remaining 15. We also require 2 
 * successful acks in a row (paranoia). The mbd kernel actually needs 
 * this. 
 */
timeout = 500;
for (i=ackcnt=0;i<4+15;i++) {
	ch = ATTN;
	PUT_BYTES(&ch,1);
	if (i == 4) timeout = 1000;
	if (GET_BYTE(&ch,timeout) && ch == ACK) {
		ackcnt++;
		printDiag(1,"%02x ",ch);
		if (ackcnt >= 2) break;
		}
	else ackcnt = 0;
	}
if (i >= 4+15) { 
	printDiag(0,"resync failure.\n");
	resync_active = 0;
	return(0);
	}
printDiag(1,"resync achieved.\n");
target_stopped = 2;

if (force_getsap && !checkSAP()) {
	deleteOcmRec((Ulong)savearea.sap);
	savearea.sap = 0;
	if (!getSAP()) return(0);
	}
resync_active = 0;
return(1);
}

/*************************************************************
*  int send_instr(Ulong v) DEF
*/
int send_instr(Ulong v)
{
char buf[100];

printDiag(2,"send_instr(%08x)\n",v);

if (diaglevel >= 4 && v != 0x12345678 && v != 0x87654321 && v != 0xdeadbeef) {
	disasm(buf,instr_buffer_cnt*4,v);
	printDiag(4,buf);
	printDiag(4,"\n");
	}

#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode) {
	if (v == 0x87654321) { /* RUN_MODE */
		if (!EjtagControl(EJTAG_RUN,0,0)) {
			printDiag(1,"EjtagControl: RUN: failed.\n");
			return(0);
			}
		}
	else {
		if (!EjtagControl(EJTAG_WRITE,v,0)) {
			printDiag(1,"EjtagControl: WRITE: failed.\n");
			return(0);
			}
		}
	return(1);
	}
#endif

instr_buffer[instr_buffer_cnt++] = v;
if (lendian_host) swap32n((Uchar *)&v,1);
PUT_BYTES((Uchar *)&v,4);
if (instr_buffer_cnt == 1) printDiag(2,"buffer_cnt="); 
printDiag(2,"%d ",instr_buffer_cnt); 
printDiag(3,"%08x ",v);
return(1);
}

/*************************************************************
*  U64 readWord(void) DEF
*/
U64 readWord(void)
{
int i;
U64 v;
Uchar ch;

v.flags = 0;
v.lo = 0;
for (i=0;i<4;i++) {
	if (!GET_BYTE(&ch,1000)) {
		printDiag(0,"readWord: timeout\n");
		return(v);
		}
	v.lo <<= 8;
	v.lo |= ch;
	}
v.lo =  (v.lo<<24)|((v.lo<<8)&0x00ff0000)|((v.lo>>8)&0x0000ff00)|(v.lo>>24);

if (mode_64bit) {
	v.flags |= U64_64;
	v.hi = 0;
	for (i=0;i<4;i++) {
		if (!GET_BYTE(&ch,1000)) {
			printDiag(0,"readWord: timeout\n");
			return(v);
			}
		v.hi <<= 8;
		v.hi |= ch;
		}
	v.hi =  (v.hi<<24)|((v.hi<<8)&0x00ff0000)|((v.hi>>8)&0x0000ff00)|(v.hi>>24);
	}

if (send_ack_each_word) PUT_BYTES((Uchar *)"0",1);

v.flags |= U64_V;
printDiag(1,"readWord=%s\n",rvtostr(v));
return(v);
}

/*************************************************************
*  Ulong *save_instr_buffer() DEF
*/
Ulong *save_instr_buffer()
{
int i;
Ulong *p;

printDiag(1,"save_instr_buffer: cnt=%d\n",instr_buffer_cnt);
if (instr_buffer_cnt == 0) return(0);

p = malloc((instr_buffer_cnt+1)*4);
p[0] = instr_buffer_cnt;

for (i=0;i<instr_buffer_cnt;i++) p[i+1] = instr_buffer[i];
instr_buffer_cnt = 0;
return(p);
}

/*************************************************************
*  void restore_instr_buffer(Ulong *p) DEF
*/
void restore_instr_buffer(Ulong *p)
{
int i;

if (p==0) return;
printDiag(1,"restore_instr_buffer cnt=%d\n",p[0]);

instr_buffer_cnt = p[0];
for (i=0;i<instr_buffer_cnt;i++) instr_buffer[i] = p[i+1];
free(p);
}

/*************************************************************
*  U64 readA0(void) DEF
*/
U64 readA0(void)
{
U64 rv;
int i,n,sbc;
Ulong *buf,v[2];

if (mode_64bit) rv.flags = U64_64;
else rv.flags = 0;

#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode) {
	if (!send_instr(0)) return(rv); /* nop */
	if (mode_64bit) {
		if (!EjtagControl(EJTAG_EXE64,0,&v)) {
			printDiag(0,"EjtagControl: EXE: failed.\n");
			return(rv);
			}
		rv.flags = U64_V|U64_64;
		rv.lo = v[1];
		rv.hi = v[0];
		}
	else {
		if (!EjtagControl(EJTAG_EXE,0,&v)) {
			printDiag(0,"EjtagControl: EXE: failed.\n");
			return(rv);
			}
		rv = mkRV(v[0]);
		}
	printDiag(2,"v = %s\n",rvtostr(rv));
	return(rv);
	}
#endif

flushInput();
if (!send_instr(SENDA0)) return(rv);
if (savearea.ibs && instr_buffer_cnt > ((int)savearea.ibs)) {
	printDiag(0,"ibuf overflow ibc=%d\n",instr_buffer_cnt);
	instr_buffer_cnt = 0;
	return(rv);
	}

for (n=0;;n++) {
	rv = readWord();
	if (rv.flags&U64_V) break;
	printDiag(1,"readA0: readWord failed\n");
	if (!resyncOk) { rv = mkRV(0); break;}
	if (n >= MAXRETRIES) {rv=mkRV(0); break;}
	printDiag(1,"readA0: resending...\n");
	buf = save_instr_buffer();
	resync();
	restore_instr_buffer(buf);
	sbc = instr_buffer_cnt;
	instr_buffer_cnt = 0;
	for (i=0;i<sbc;i++) if (!send_instr(instr_buffer[i])) return(rv);
	}

instr_buffer_cnt = 0;
if (force_getsap) {
	target_stopped = 1; /* force wakeup at start of each command */
	deleteOcmRec((Ulong)savearea.sap);
	savearea.sap = 0; /* force reread of SAP */
	}

return(rv);
}

/*************************************************************
*  U64 read_target(int type,Ulong addr,int sz) DEF
*/
U64 read_target(int type,Ulong addr,int sz)
{
int reg,offs,diff;
OcmRec *p;
U64 v,v0,v1;

printDiag(1,"read_target(%s,%08x,%d) ",xtcodes[type],addr,sz);

if (mode_64bit) v.flags = U64_64;
else v.flags = 0;

if (!send_buffer()) return(v);
reg = addr;
switch (type) {
	case XT_PC :
		switch (reg) {
		   case 0 : /* PC */
			if (!getSAP()) return(v);
			return read_target(XT_MEM,
				(Ulong)(ICE_EPC+savearea.sap),4);
		   case 1 : /* HI */
			if (!send_instr(OP_MFHI|(4<<11))) return(v);
			return readA0();
		   case 2 : /* LO */
			if (!send_instr(OP_MFLO|(4<<11))) return(v);
			return readA0();
		   }
	case XT_MEM :
		if ((p=is_ocm(addr)) && p->func) {
			return run_ocm(p,0,addr,sz,v.lo);
			}
		/* unaligned operations */
		if (sz == 2 && addr&1) { /* byte aligned */
			v0 = read_target(XT_MEM,addr,1);
			if (!(v0.flags&U64_V)) return(v0);
			v1 = read_target(XT_MEM,addr+1,1);
			if (!(v1.flags&U64_V)) return(v1);
			v.hi = 0;
			v.lo = v0.lo<<8;
			v.lo |= v1.lo;
			v.flags |= U64_V;
			return(v);
			}
		if (sz == 4 && addr&2) { /* half-word aligned */
			addr &= ~1; /* clear the LS bit */
			v0 = read_target(XT_MEM,addr,2);
			if (!(v0.flags&U64_V)) return(v0);
			v1 = read_target(XT_MEM,addr+2,2);
			if (!(v1.flags&U64_V)) return(v1);
			v.hi = 0;
			v.lo = v0.lo<<16;
			v.lo |= v1.lo;
			v.flags |= U64_V;
			return(v);
			}
		if (sz == 4 && addr&1) { /* byte aligned */
			addr &= ~1; /* clear the LS bit */
			return read_target(XT_MEM,addr,4);
			}
		if (sz == 8 && addr&4) { /* word aligned */
			v0 = read_target(XT_MEM,addr,4);
			if (!(v0.flags&U64_V)) return(v0);
			v1 = read_target(XT_MEM,addr+4,4);
			if (!(v1.flags&U64_V)) return(v1);
			v.hi = v0.lo;
			v.lo = v1.lo;
			v.flags |= U64_V;
			return(v);
			}
#if 1 /* 990324 */
		if (is_cacheable(addr)) {
			if (dstate==stale) {
				flush_target(DCACHEI);
				dstate = clean;
				}
			}
		else {
			if (has_wb_dcache && dstate==dirty) {
				flush_target(DCACHE);
				dstate = clean;
				}
			}
#endif
		offs=0;
		diff = addr-current_r9.lo;
		if (diff < -32768 || diff > 32767) writeGpri(9,addr);
		else offs = (addr-current_r9.lo)&0xffff;
		switch (sz) {
			case 1 : 
				if (!send_instr(OP_LBU|(4<<16)|(9<<21)|offs))
					return(v); 
				break;
			case 2 : 
				if (!send_instr(OP_LHU|(4<<16)|(9<<21)|offs))
					return(v); 
				break;
			case 4 : 
				if (!send_instr(LW(4,offs,9))) return(v); 
				break;
			case 8 : 
				if (!send_instr(LD(4,offs,9))) return(v); 
				break;
			}
		return readA0();
	case XT_GPR :
		return readUgpr(reg);
	case XT_CP0 : 
		if (!send_instr(MFC0(4,reg))) return(v);
		return readA0();
	case XT_CP1 :
		if (!send_instr(OP_MFC1|(4<<16)|(reg<<11))) return(v);
		return readA0();
	case XT_CP1C :
		if (!send_instr(OP_CFC1|(4<<16)|(reg<<11))) return(v);
		return readA0();
	case XT_DBX :
		if (!send_instr(MFD(4,reg))) return(v);
		return readA0();
		break;
	case XT_CP2 :
	case XT_CP2C :
	case XT_CP3 : 
	case XT_CP3C : 
	default : printDiag(0,"%08x: read_target: bad type\n",type);
	}
return(v);
}

/*************************************************************
*  U64 readUgpr(int reg) DEF
*	Read user's GP register
*/
U64 readUgpr(int reg)
{
Ulong msk;
int i,n,sz;
U64 v; /* used for error returns only */

printDiag(1,"readUgpr(%d)\n",reg);

if (mode_64bit) {
	sz = 8;
	v.flags = U64_64;
	}
else {
	sz = 4;
	v.flags = 0;
	}

if (!getSAP()) return(v);
if (!send_buffer()) return(v);

if (reg == 32) {
	if (!send_instr(OP_MFLO|(4<<11))) return(v);
	return readA0();
	}
else if (reg == 33) {
	if (!send_instr(OP_MFHI|(4<<11))) return(v);
	return readA0();
	}
else if (savearea.map&(1<<reg)) { /* reg is in save area */
	msk = 1;
	for (i=n=0;i<reg;i++) {
		if (savearea.map&msk) n++;
		msk <<= 1;
		}
	if (mode_64bit) n *= 2;
	printDiag(1,"readUgpr hsize=%d n=%d\n",savearea.hsize,n); 
	return read_target(XT_MEM,(Ulong)(savearea.hsize+n+savearea.sap),sz);
	}
else { /* reg not in save area */
	if (!send_instr(OP_OR|(4<<11)|(0<<16)|(reg<<21))) return(v);
	return readA0();
	}
return(v);
}

/*************************************************************
*  int writeUgpr(int reg,U64 v) DEF
*	Write user's GP register
*/
int writeUgpr(int reg,U64 v)
{
Ulong msk;
int i,n,sz;

printDiag(1,"writeUgpr(%d,%s)\n",reg,rvtostr(v));

if (mode_64bit) sz = 8;
else sz = 4;

if (!getSAP()) return(0);
if (!send_buffer()) return(0);

if (reg == 32) {
	if (!writeGpr(8,v)) return(0);
	send_instr(OP_MTLO|(8<<21));
	if (!(readA0().flags&U64_V)) return(0);
	}
else if (reg == 33) {
	if (!writeGpr(8,v)) return(0);
	send_instr(OP_MTHI|(8<<21));
	if (!(readA0().flags&U64_V)) return(0);
	}
else if (savearea.map&(1<<reg)) { /* reg is in save area */
	msk = 1;
	for (i=n=0;i<reg;i++) {
		if (savearea.map&msk) n++;
		msk <<= 1;
		}
	if (mode_64bit) n *= 2;
	if (!write_target(XT_MEM,(Ulong)(savearea.hsize+n+savearea.sap),v,sz))
		return(0);
	}
else { /* reg not in save area */
	if (!writeGpr(reg,v)) return(0);
	if (!(readA0().flags&U64_V)) return(0);
	}
return(1);
}

#define bitset(v,n)	(((v)>>(n))&1)

/*************************************************************
*  int writeGpri(int reg,Ulong cv) DEF
*/
int writeGpri(int reg,Ulong cv)
{
U64 v;

if (mode_64bit) {
	v.flags = (U64_64|U64_V);
	if (bitset(cv,31)) v.hi = 0xffffffff;
	else v.hi = 0;
	}
else v.flags = U64_V;

v.lo = cv;
return writeGpr(reg,v);
}

/*************************************************************
*  int writeGpr(int reg,U64 v) DEF
*	This has been tested. But not with real hardware.
*/
int writeGpr(int reg,U64 v)
{
long n;
int t,cnt,shamt;

printDiag(2,"writeGpr(%d,%s)\n",reg,rvtostr(v));
if (reg == 8) current_r8 = v;
else if (reg == 9) current_r9 = v;

if (!(v.flags&U64_V)) return(0); /* error */
if (v.flags&U64_64 && !mode_64bit) return(0); /* error */

if (!send_buffer()) return(0);

cnt = 0;
n = (long)v.lo;
if (v.flags&U64_64) {
	if ((v.hi == 0xffffffff && (v.lo>>16) == 0xffff && bitset(v.lo,15)) ||
	     (v.hi == 0 && (v.lo>>16) == 0 && !bitset(v.lo,15))) {
		// 64-bit value is grouped into 4 blocks of 16 bits
		// 4f 4f 4f  bit15 addiu
		//  0  0  0 !bit15 addiu
		// 4f=ffff 0=0000 bit15=bit15isset
		if (!send_instr(ADDIU(reg,0,v.lo&0xffff))) return(0);
		cnt++;
		}
	else if ((v.hi == 0xffffffff && bitset(v.lo,31)) ||
		 (v.hi == 0 && !bitset(v.lo,31))) {
		 // 4f 4f  bit15 nz lui,ori
		 // 4f 4f  bit15 0  lui
		 //  0  0 !bit15 nz lui,ori
		 //  0  0 !bit15 0  lui
		 // nz = non zero value
		 if (!send_instr(LUI(reg,v.lo>>16))) return(0);
		 cnt++;
		 if ((v.lo&0xffff) != 0) { // ori
			if (!send_instr(ORI(reg,reg,v.lo&0xffff))) return(0);
			cnt++;
		 	}
		 }
	else if (((v.hi>>16) == 0xffff && bitset(v.hi,15)) ||
	         ((v.hi>>16) == 0 && !bitset(v.hi,15))) {
		 // 4f  bit15 nz  0  lui,ori,dsll
		 // 4f  bit15 nz nz  lui,ori,dsll,ori
		 // 4f  bit15  0  0  lui,dsll
		 // 4f  bit15  0 nz  lui,dsll,ori
		 //  0 !bit15 nz  0  lui,ori,dsll
		 //  0 !bit15 nz nz  lui,ori,dsll,ori
		 //  0 !bit15  0  0  lui,dsll
		 //  0 !bit15  0 nz  lui,dsll,ori
		 if (!send_instr(LUI(reg,v.hi&0xffff))) return(0);
		 cnt++;
		 if ((v.lo>>16) != 0) {
			if (!send_instr(ORI(reg,reg,v.lo>>16))) return(0);
			cnt++;
		 	}
		 if (!send_instr(DSLL(reg,reg,16))) return(0);
		 cnt++;
		 if ((v.lo&0xffff) != 0) { // ori
			if (!send_instr(ORI(reg,reg,v.lo&0xffff))) return(0);
			cnt++;
		 	}
		}
	else { /* assumes ((v.hi>>16) != 0) */
		 // nz 0  nz 0  lui,dsll,ori,dsll
		 // nz 0  nz nz lui,dsll,ori,dsl,ori
		 // nz 0   0 0  lui,dsll32
		 // nz 0   0 nz lui,dsll32,ori
		 // nz nz nz 0  lui,ori,dsll,ori,dsll
		 // nz nz nz nz lui,ori,dsll,ori,dsl,ori
		 // nz nz  0 0  lui,ori,dsll32
		 // nz nz  0 nz lui,ori,dsll32,ori
		 if (!send_instr(LUI(reg,v.hi>>16))) return(0);
		 cnt++;
		 if ((v.hi&0xffff) != 0) { // ori
			if (!send_instr(ORI(reg,reg,v.hi&0xffff))) return(0);
			cnt++;
		 	}
		 if ((v.lo>>16) == 0) shamt = 32;
		 else shamt = 16;
		 if (!send_instr(DSLL(reg,reg,shamt))) return(0);
		 cnt++;
		 if ((v.lo>>16) != 0) {
			if (!send_instr(ORI(reg,reg,v.lo>>16))) return(0);
		 	cnt++;
		 	if (!send_instr(DSLL(reg,reg,16))) return(0);
		 	cnt++;
		 	}
		 if ((v.lo&0xffff) != 0) {
			if (!send_instr(ORI(reg,reg,v.lo&0xffff))) return(0);
		 	cnt++;
		 	}
		}
	}
else { // 32-bit mode
	n = (long)v.lo;
	t = cnt = 0;
	if (n >= -32768 && n <= 32767) {
		if (!send_instr(ADDIU(reg,0,v.lo&0xffff))) return(0);
		cnt++;
		}
	else {
		/* 990304 removed if(v.lo&0xffff0000) */
		if (!send_instr(LUI(reg,v.lo>>16))) return(0);
		t = reg;
		cnt++;
		if (v.lo&0xffff) {
			if (!send_instr(ORI(reg,reg,v.lo&0xffff))) return(0);
			cnt++;
			}
		}
	}
return(cnt);
}

/*************************************************************
*  void swap32n(Uchar *p,int n) DEF
*/
void swap32n(Uchar *p,int n)
{
Uchar v;

for (;n>0;n--) {
	v = p[0]; p[0] = p[3]; p[3] = v;
	v = p[1]; p[1] = p[2]; p[2] = v;
	p += 4;
	}
}

/*************************************************************
*  Ulong read_unaligned_long(Uchar *a) DEF
*/
Ulong read_unaligned_long(Uchar *a)
{
#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode)
	return (((a)[3]<<24)|((a)[2]<<16)| ((a)[1]<<8)|((a)[0])); 
#endif
return (((a)[0]<<24)|((a)[1]<<16)| ((a)[2]<<8)|((a)[3])); 
}

/*************************************************************
*  void flushInput(void) DEF
*/
void flushInput(void)
{
int i;
Uchar ch;

for (i=0;;i++) {
	if (!GET_BYTE(&ch,1)) break;
	if (i==0) printDiag(2,"flushInput ");
	printDiag(9,"%02x ",ch);
	}
if (i) printDiag(1,"%d bytes flushed.\n",i);
}

/*************************************************************
*  int checkSAP(void) DEF
*/
int checkSAP(void)
{
Ulong v;
U64 rv;

printDiag(1,"checkSAP ");
flushInput();
for (;;) {
	if (!send_instr(SENDSAP)) {
		printDiag(1,"checkSAP: send_instr failed\n");
		return(0);
		}
	rv = readWord();
	if (rv.flags&U64_V) break;
	}
v = rv.lo& ~1;
v |= K1BASE;
instr_buffer_cnt = 0;
if (savearea.sap && v == (Ulong)savearea.sap) return(1);
printDiag(1,"checkSAP: need new SAP value %08x vs %s\n",savearea.sap,
	rvtostr(rv));
return(0);
}

/*************************************************************
*  int getSAP(void) DEF
*/
int getSAP(void)
{
Ulong v,sav,m;
int sz,i;
static int getsap_active;
U64 rv;

/* avoid unintended recursion and unnecessary operations */
if (getsap_active || savearea.sap) return(1); 
getsap_active = 1;

printDiag(1,"getSAP ");

current_r8 = current_r9 = mkRV(0);
if (target_stopped != 2 && !wakeup()) return(0); /* 980407 */

#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode) {
	printDiag(4,"EjtagControl(SAP)\n");
	if (!EjtagControl(EJTAG_SAP,0,&v)) {
		printf("EjtagControl: SAP: failed.\n");
		getsap_active = 0;
		return(0);
		}
	rv = mkRV(v);
	printDiag(2,"sap = %s\n",rvtostr(rv));
	}
else {
#endif
	flushInput();
	for (;;) {
		instr_buffer_cnt = 0;
		if (!send_instr(SENDSAP)) return(0);
		rv = readWord();
		if (rv.flags&U64_V) break;
		}
#if defined(EJTAG) && !defined(PMCC)
	}
#endif
instr_buffer_cnt = 0;
v = K1BASE|rv.lo;
target_stopped = 2;

if (v&1) {
	printDiag(1,"new style savearea\n");
	v &= ~1;
	savearea.sap = (Ulong *)v;
	/* need to read header info */
	/* check to see if it's the right version */
	rv=read_target(XT_MEM,(Ulong)(ICE_SAV+savearea.sap),4);
	if (!(rv.flags&U64_V)) {
		printDiag(0,"getSAP: SAV read from %08x failed.\n",
			ICE_SAV+savearea.sap);
		return(0);
		}
	sav = rv.lo;
	if (sav != 2 && sav != 3) {
		printDiag(1,"bad savearea version %d\n",sav);
		savearea.sap = 0;
		getsap_active = 0;
		return(0); /* wrong version */
		}
	savearea.vers = sav;
	if (sav == 3) mode_64bit = 1;
	resyncOk = 1;
	savearea.hsize = read_target(XT_MEM,(Ulong)(ICE_SAH+savearea.sap),4).lo;
	savearea.map = read_target(XT_MEM,(Ulong)(ICE_MAP+savearea.sap),4).lo;
	savearea.ibs = read_target(XT_MEM,(Ulong)(ICE_IBS+savearea.sap),4).lo;
	if (savearea.hsize > 7)
	    savearea.le = read_target(XT_MEM,(Ulong)(ICE_LE+savearea.sap),4).lo;
	printDiag(1,"getSAP vers=%d hsize=%d map=%08x ibs=%d le=%d\n",
			savearea.vers,savearea.hsize,savearea.map,
			savearea.ibs,savearea.le);
	/* 980303 turn off flush requirement when writing to savearea */
	sz = savearea.hsize;
	for (i=0,m=1;i<32;i++,m<<=1) if (m&savearea.map) sz++;
	addOcmRec((Ulong)savearea.sap,sz*4,0,0);
	}
else {
	printDiag(1,"old style savearea\n");
	savearea.sap = (Ulong *)v;
	savearea.vers = 1;
	addOcmRec((Ulong)savearea.sap,12*4,0,0);
	}

#if 1
if (need_initial_flush) {
	printDiag(1,"performing initial flush\n");
        need_initial_flush = 0;
        if (!flush_target(DCACHEI)) return(0);
        if (!flush_target(ICACHEI)) return(0);
        }
#endif

getsap_active = 0;
return(1);
}

/*************************************************************
*  int hwibReq(Ulong addr) DEF
*	verify that other hw ibpts don't conflict with this one.
*	i.e. More than one. Return -1 if not possible.
*	Else return hw resource number.
*/
int hwibReq(addr)
Ulong addr;
{
int i,n;

/* if (is_ocm(addr)) return(-1); 980805 */
if (!has_nhwib) return(-1);

n = 0;
/* count how many are already set */
for (i=0;i<MAX_BPT;i++) {
	if (brkList[i].type==0) continue;
	if (brkList[i].method != BRK_METHOD_HW) continue;
	if (brkList[i].type == BPTYPE_DATA) continue;
	n++;
	}
if (n > (has_nhwib-1)) return(-1);
return(n);
}

/*************************************************************
*  int hwdbReq(Ulong addr) DEF
*	verify that other hw dbpts don't conflict with this one.
*	i.e. More than one. Return -1 if not possible.
*	Else return hw resource number.
*/
int hwdbReq(addr)
Ulong addr;
{
int i,n;

/* if (is_ocm(addr)) return(-1); 980805 */
if (!has_nhwdb) return(-1);

n = 0;
for (i=0;i<MAX_BPT;i++) {
	if (brkList[i].type==0) continue;
	if (brkList[i].method != BRK_METHOD_HW) continue;
	if (brkList[i].type != BPTYPE_DATA) continue;
	n++;
	}
if (n > (has_nhwdb-1)) return(-1);
return(n);
}

/*************************************************************
*  void printBpts() DEF
*/
void printBpts()
{
int i;

for (i=0;i<MAX_BPT;i++) {
	if (brkList[i].type==0) continue;
	switch (brkList[i].method) {
		case BRK_METHOD_HW :
			printDiag(1,"BRK_METHOD_HW %08x\n",brkList[i].addr);
			break;
		case BRK_METHOD_RAM :
			printDiag(1,"BRK_METHOD_RAM %08x\n",brkList[i].addr);
			break;
		case BRK_METHOD_ROM :
			printDiag(1,"BRK_METHOD_ROM %08x\n",brkList[i].addr);
			break;
		default: 
			printDiag(0,"%d: BRK_METHOD unknown\n",
				brkList[i].method);
		}
	}
}

/*************************************************************
*  int is_cacheable(addr) DEF
*/
int is_cacheable(addr)
Ulong addr;
{
if (IS_K0SEG(addr)) return(1);
return(0);
}

/*************************************************************
*  int setbp_target(int n,int type,Ulong addr,Ulong addr2,Ulong value) DEF
*	type: 1=bpc 2=bda 3=itemp 4=sstep 5=nonrt
*	returns bp number, or -1 on error.
*/
int setbp_target(n,type,addr,addr2,value)
int n,type;
Ulong addr,addr2,value;
{
int i,method,atype,code;
Ulong info,x;
long mask; /* must be signed */

printDiag(1,"setbp_target(%d,%x,%08x,%08x,%08x)\n",n,type,addr,addr2,value); 
atype = type>>4;
type &= 0xf;
code = 0;
if (type == BPTYPE_NONRT) {
	printDiag(0,"warning: this breakpoint requires non real-time execution\n");
	nonrt = 1;
	return(0);
	}

if ((n=which_bpt(addr,type)) != -1) {
	printDiag(1,"setbp_target: %08x duplicate bpt\n",addr); 
	return(0-BP_E_ERR);
	}

if (type == BPTYPE_PC || type == BPTYPE_ITMP || type == BPTYPE_TRACE) {
	if (is_writeable_target(addr)) method = BRK_METHOD_RAM;
	else if (is_cacheable(addr) && ilockReq(addr)) method = BRK_METHOD_ROM;
	else if ((x=hwibReq(addr)) != -1) {
                method = BRK_METHOD_HW;
		if (atype&8) mask = addr2; /* 8 = addr2 is a mask */
		else if (addr2) { /* compute mask */
			for (mask=0x80000000;(addr&mask) == (addr2&mask);mask>>=1) ;
			mask <<= 1; /* oops! one step too many */
			code |= BP_W_MSK;
			}
		else mask = 0xffffffff; /* all-enable */
#if 0 /* 981214 */
                dcs = DCS_TR|DCS_UD|DCS_KD|DCS_DE|DCS_PCE;
		/* Note: some dbx implementations require a Physical Address */
#endif
		info = (x<<16);
                }
	else {
		printDiag(0,"%08x: can't set bpt\n",addr);
		return(0-BP_E_ERR);
		}
	}
else if (type == BPTYPE_DATA) {
	if ((x=hwdbReq(addr)) == -1) {
		printDiag(0,"%08x: can't set bpt\n",addr);
		return(0-BP_E_ERR);
		}
        method = BRK_METHOD_HW;
#if 0 /* 981214 */
        dcs = DCS_TR|DCS_UD|DCS_KD|DCS_DE|DCS_DAE;
 
        if (atype&1) dcs |= DCS_DR; /* 1 = read */
        if (atype&2) dcs |= DCS_DW; /* 2 = write */
#else
	info = (x<<16);
        if (atype&1) info |= 2; /* 2 = read */
        if (atype&2) info |= 1; /* 1 = write */
#endif

	if (atype&4) return(0-BP_E_VAL);
	/* 4 is used by the gdb interface to indicate that a value
	 * has been specified. See debug.c. 
	 */
		
 
	if (atype&8) mask = addr2; /* 8 = addr2 is a mask */
	else if (addr2) { /* compute mask */
                for (mask=0x80000000;(addr&mask) == (addr2&mask);mask>>=1) ;
                mask <<= 1; /* oops! one step too many */
		code |= BP_W_MSK;
                }
        else mask = 0xffffffff; /* all-enable */
	printDiag(3,"setbp_target: dbpt addr=%08x mask=%08x\n",addr,mask);
        }

if (n == -1) {
	for (i=0;i<MAX_BPT;i++) if (brkList[i].type == 0) break;
	if (i >= MAX_BPT) {
		printDiag(0,"fatal error: out of bpts\n");
		return(0-BP_E_ERR);
		}
	n = i;
	}
if (n < 0 || n >= MAX_BPT) {
	printDiag(0,"%d: bad bpt number\n",n);
	return(0-BP_E_ERR);
	}
brkList[n].type = type;
brkList[n].addr = addr;
brkList[n].method = method;
brkList[n].mask = mask;
brkList[n].aux[0] = info;
return((code<<16)|n);
}

/*************************************************************
*  int clrbp_target(int n) DEF
*/
int clrbp_target(n)
int n;
{
printDiag(1,"clrbp_target(%d)\n",n);
if (n < 0 || n >= MAX_BPT) {
	printDiag(0,"%d: bad bpt number\n",n);
	return(0);
	}
if (brkList[n].type==0) {
	printDiag(0,"%d: bpt is not set\n",n);
	return(0);
	}
brkList[n].type = 0;
return(1);
}


/*************************************************************
*/
int which_bpt(Ulong addr,int type)
{
int i;

for (i=0;i<MAX_BPT;i++) {
	if (brkList[i].type==0) continue;
	if (type && type != brkList[i].type) continue;
	if (brkList[i].addr == addr) return(i);
	}
return(-1);
}

/*************************************************************
*  int which_dbpt(int hwbptno) DEF
*/
int which_dbpt(int hwbptno)
{
int i;

for (i=0;i<MAX_BPT;i++) {
        if (brkList[i].type==0) continue;
        if (brkList[i].type != BPTYPE_DATA) continue;
        if (brkList[i].method != BRK_METHOD_HW) continue;
        if (((int)(brkList[i].aux[0]>>16)) == hwbptno) return(i);
        }
return(-1);
}

#define RS_(x)     (((x) >> 21) & ((1L <<  5) - 1))
#define SEX16(x)   ((long)(x<<16)>>16)
#define SIMM16(x)  (SEX16(UIMM16(x)))
#define UIMM16(x)  ((x)&0xffff)
#define IS_LDST(n) ((((n)>>26)>=0x20)&&(((n)>>26)<=0x2e))

/*************************************************************
*  int is_bpt(addr) DEF
*/
int is_bpt(addr)
Ulong addr;
{
int i;
Ulong instr,ea;

for (i=0;i<MAX_BPT;i++) {
	if (brkList[i].type==0) continue;
	if (brkList[i].addr == addr) return(brkList[i].type);
	if (brkList[i].type == BPTYPE_DATA) {
		instr = read_target(XT_MEM,addr,4).lo; /* read instr */
		/* if lw/sw and addr eval == addr return type */
		if (!IS_LDST(instr)) continue;
		ea = read_target(XT_GPR,RS_(instr),4).lo + SIMM16(instr);
		if ((ea&brkList[i].mask) == brkList[i].addr) {
			printDiag(2,"is_bpt: %08x will hwdb\n",addr);
			return(brkList[i].type);
			}
		}
	}
return(0);
}


/*************************************************************
*  void brkDelete(Ulong type) DEF
*/
void brkDelete(type)
Ulong type;
{
int i;

for (i=0;i<MAX_BPT;i++) {
	if (type == brkList[i].type) brkList[i].type = 0;
	}
}

/*************************************************************
*  int is_writeable_target(Ulong adr) DEF
*/
int is_writeable_target(adr)
Ulong adr;
{
Uchar x,y;
OcmRec *p;
U64 v;

if ((p=is_ocm(adr)) && p->readonly) {
        printDiag(1,"\nocm not writeable(%08x)\n",adr);
        return(0);
        }

adr |= K1BASE;
v = read_target(XT_MEM,adr,1);
x = (Uchar)v.lo;
y = ~x;

if (!writeGpri(8,y)) return(0);
if (!writeGpri(9,adr)) return(0);
if (!send_instr(SB(8,0,9))) return(0);
if (!(readA0().flags&U64_V)) return(0); /* error condition

/* flush_target(DCACHE); */
v = read_target(XT_MEM,adr,1);
if (((Uchar)v.lo) != y) {
        printDiag(1,"\nnot writeable(%08x)\n",adr);
	return(0);
	}

printDiag(1,"%08x is writeable\n",adr);
/* restore the value */
if (!writeGpri(8,x)) return(0);
if (!writeGpri(9,adr)) return(0);
if (!send_instr(SB(8,0,9))) return(0);
if (!(readA0().flags&U64_V)) return(0); /* error condition */

return(1);
}


/*************************************************************
*  int send_buffer(void) DEF
*	write_target operations simply write to a buffer. Then
*	from time to time the buffer gets flushed down to the
*	target by calling this function.
*/
int send_buffer(void)
{
int wds;
Ulong addr,eadr,v;
static send_buffer_active;
Uchar *p;

if (send_buffer_active) return(1);
if (target_stopped != 2 && !wakeup()) return(0);

if (ice.buf==0) ice.buf = (Uchar *)malloc(BUFSZ);
if (ice.bcnt==0 && ice.rlcnt==0) return(1); /* nothing to do */
send_buffer_active=1;
printDiag(3,"send_buffer()\n");

if (is_cacheable(ice.start_addr)) dstate = dirty;
else dstate = stale;

printDiag(1,"send_buffer: sadr=%08x bcnt=%d rlcnt=%d\n",
	ice.start_addr,ice.bcnt,ice.rlcnt);

if (ice.rlcnt && ice.rlcnt <= RLTHRESHOLD) /* expand rlcnt in buffer */
	for (;ice.rlcnt;ice.rlcnt--) ice.buf[ice.bcnt++] = ice.prev_v;

addr = ice.start_addr;
eadr = addr+ice.bcnt;
p = ice.buf;

if (addr == 0xbfff0000 && ice.bcnt == 4)
	printDiag(1,"[%02x,%02x,%02x,%02x]\n",p[0],p[1],p[2],p[3]);

/* send bytes until word aligned address */
while (addr&3 && addr < eadr) if (!send_byte(addr++,*p++)) return(0);

/* compute number of complete words */
if (addr == eadr) wds = 0;
else wds = ((eadr&~3) - addr)/4;

printDiag(1,"send_buffer: eadr=%08x wds=%d\n",eadr,wds);

if (wds > 3) {
#if defined(EJTAG) && !defined(PMCC)
	if (ejtag_mode) swap32n(p,wds);
#endif
	if (lendian_target) swap32n(p,wds);
	if (!send_words(addr,(Ulong *)p,wds)) return(0);
	addr += wds*4;
	p += wds*4;
	}
else for (;wds;wds--,addr += 4,p += 4) {
	v = read_unaligned_long(p);
#if defined(EJTAG) && !defined(PMCC)
	if (ejtag_mode) swap32n((Uchar *)&v,1);
#endif
	if (lendian_target) swap32n((Uchar *)&v,1);
	if (!send_bhw(addr,v,4)) return(0);
	}

/* send trailing bytes */
while (addr < eadr) if (!send_byte(addr++,*p++)) return(0);

if (ice.rlcnt > RLTHRESHOLD && !set_bytes(addr,ice.prev_v,ice.rlcnt)) return(0);

ice.rlcnt = ice.bcnt = 0;
setFlushneeded(ice.start_addr,ice.rlcnt+ice.bcnt);
send_buffer_active=0;
return(1);
}

/*************************************************************
*  int write_target_byte(Ulong addr,Ulong v) DEF
*	write_target() calls this function to add bytes to the
*	buffer. send_buffer() is used to actually send the bytes
*	to the target.
*/
int write_target_byte(addr,v)
Ulong addr,v;
{

printDiag(5,"write_target_byte(%08x,%02x)\n",addr,v&0xff);
if (addr != ice.expected_addr && !send_buffer()) return(0);

v &= 0xff;
if (v == ice.prev_v && ice.bcnt) {
	ice.rlcnt++;
	if (ice.rlcnt >= MAXRL && !send_buffer()) return(0);
	}
else {
	if (ice.rlcnt) {
		if (ice.rlcnt <= RLTHRESHOLD) { /* expand in place */
			for (;ice.rlcnt;ice.rlcnt--) 
				ice.buf[ice.bcnt++] = ice.prev_v;
			if (ice.bcnt >= BUFSZ && !send_buffer()) return(0);
			}
		else if (!send_buffer()) return(0);
		}
	if (ice.bcnt == 0) {
		ice.start_addr = addr;
		ice.sz = 1;
		}
	ice.buf[ice.bcnt++] = ice.prev_v = (Uchar)v;
	if (ice.bcnt+RLTHRESHOLD >= BUFSZ && !send_buffer()) return(0);
	}
ice.expected_addr = addr+1;
return(1);
}

/*************************************************************
*  int send_byte(Ulong addr,Uchar v) DEF
*/
int send_byte(Ulong addr,Uchar v)
{
int offs,diff;
OcmRec *p;

printDiag(5,"send_byte(%08x,%02x)\n",addr,v);

if ((p=is_ocm(addr)) && p->func) {
	if (!(run_ocm(p,1,addr,1,v).flags&U64_V)) return(0);
	return(1);
	}

offs=0;
diff = addr-current_r9.lo;
if (diff < -32768 || diff > 32767) {
	if (!writeGpri(9,addr)) return(0);
	}
else offs = (addr-current_r9.lo)&0xffff;

if (!writeGpri(8,v)) return(0);
if (!send_instr(SB(8,offs,9))) return(0);
if (!(readA0().flags&U64_V)) return(0);
return(1);
}


/*************************************************************
*  int set_bytes(Ulong addr,Uchar v,int n) DEF
*	Set multiple bytes e.g. fill memory
*	May also be used during download.
*/
int set_bytes(Ulong addr,Uchar v,int n)
{
OcmRec *p;
int offset;

printDiag(4,"set_bytes(%08x,%02x,%d)\n",addr,v,n);

#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode) {
	if (addr&3) {
		writeGpri(9,addr);
		writeGpri(2,v);
		for (offset=0;addr&3;addr++,offset++,n--) {
			send_instr(SB(2,offset,9));
			}
		if (!(readA0().flags&U64_V)) return(0);
		}
        n /= 4; /* words not bytes */
        if (!EjtagDownload(addr,n)) {
                printDiag(0,"EjtagDownload: failed\n");
                return(0);
                }
        v = (v<<24)|(v<<16)|(v<<8)|v;
        if (!EjtagControl(EJTAG_FILL,v,0)) {
                printDiag(0,"EjtagControl: FILL: failed\n");
                return(0);
                }
	addr += n;
	n &= 3;
	if (n>0) {
		writeGpri(9,addr);
		writeGpri(2,v);
		for (offset=0;n>0;offset++,n--) {
			send_instr(SB(2,offset,9));
			}
		if (!(readA0().flags&U64_V)) return(0);
		}
        return(1);
        }
#endif


if ((p=is_ocm(addr)) && p->func) {
	for (;n>0;n--) {
		if (!(run_ocm(p,1,addr++,1,v).flags&U64_V)) return(0);
		}
	return(1);
	}

if (!writeGpri(9,addr)) return(0);
if (!writeGpri(8,n)) return(0);
if (!writeGpri(2,v)) return(0);
if (!send_instr(SB(2,0,9))) return(0);
if (!send_instr(SUBIU(8,1))) return(0);
if (!send_instr(BNE(8,0,-3))) return(0); /* self=-1 */
if (!send_instr(ADDIU(9,9,1))) return(0);
if (!(readA0().flags&U64_V)) return(0);
current_r8 = current_r9 = mkRV(0);
return(1);
}

/*************************************************************
*  int send_bhw(Ulong addr,Ulong v,int sz) DEF
*	980704 Created.
*
*	This was created to provide a quick solution for a specific
*	problem. The problem is that my write buffer discards info
*	about the size of the write access (bhw). This is not normally a
*	problem for memory. But can be a big problem for device registers.
*
*	The correct solution is to rewrite the buffer so that it remembers
*	what size data it contains. Writes of a different size would
*	cause a send_buffer(). But this is complicated to get the RL
*	encoding right for this scheme. Especially the expand-in-place.
*
*	So for now I am handling write hword as a special case. I always
*	send_buffer(), then I write the hword directly, bypassing the
*	buffer.
*/
int send_bhw(Ulong addr,Ulong v,int sz)
{
int offs,diff;
OcmRec *p;

printDiag(9,"send_bhw(%08x,%08x,%d)\n",addr,v,sz);

if ((p=is_ocm(addr)) && p->func) {
	if (!(run_ocm(p,1,addr,sz,v).flags&U64_V)) return(0);
	return(1);
	}

offs=0;
diff = addr-current_r9.lo;
if (diff < -32768 || diff > 32767) writeGpri(9,addr);
else offs = (addr-current_r9.lo)&0xffff;

if (!writeGpri(8,v)) return(0);
switch (sz) {
	case 1 : if (!send_instr(SB(8,offs,9))) return(0); break;
	case 2 : if (!send_instr(SH(8,offs,9))) return(0); break;
	case 4 : if (!send_instr(SW(8,offs,9))) return(0); break;
	}
if (!(readA0().flags&U64_V)) return(0);
return(1);
}

/*************************************************************
*  int send_words(Ulong addr,Ulong *vp,int n) DEF
*	send multiple words i.e. download
*/
int send_words(Ulong addr,Ulong *vp,int n)
{
int gwp_offs;
OcmRec *p;

printDiag(8,"send_words(%08x,%08x..,%d)\n",addr,*vp,n);

#if defined(EJTAG) && !defined(PMCC)
if (ejtag_mode) {
	printDiag(1,"EjtagDownload(%08x,%d)\n",addr,n);
	if (!EjtagDownload(addr,n)) {
		printDiag(0,"EjtagDownload: failed\n");
		return(0);
		}
	printDiag(1,"EjtagControl(BLOCK,0,%08x)\n",vp);
	if (!EjtagControl(EJTAG_BLOCK,0,vp)) {
		printDiag(0,"EjtagControl: BLOCK: failed\n");
		return(0);
		}
	return(1);
	}
#endif

if ((p=is_ocm(addr)) && p->func) {
	for (;n>0;n--,addr += 4) {
		if (!(run_ocm(p,1,addr,1,*vp++).flags&U64_V)) return(0);
		}
	return(1);
	}

if (!getSAP()) return(0);
if (savearea.vers > 1) gwp_offs = ICE_GWP;
else gwp_offs = ICE_OLD_GWP;

if (savearea.gwp==0) {
	if (savearea.sap==0) {
		printDiag(0,"fatal error: sap=0\n");
		return(0);
		}
	savearea.gwp = read_target(XT_MEM,(Ulong)(gwp_offs+savearea.sap),4).lo;
	if (savearea.gwp==0) {
		printDiag(0,"fatal error: can't get address of get_word\n");
		return(0);
		}
	printDiag(1,"gwp %08x\n",savearea.gwp);
	savearea.gwp |= K1BASE;
	}

if (!send_instr(MOVE(10,31))) return(0); /* save RA -- non-leaf */
if (!writeGpri(9,addr)) return(0);	/* start address */
if (!writeGpri(8,n)) return(0);		/* word count */
if (!writeGpri(4,savearea.gwp)) return(0);

/* 4: */
if (!send_instr(JALR_R4)) return(0);     /* jal get_word -> v0 */
if (!send_instr(SUBIU(8,1))) return(0);   /* word count-- */
if (!send_instr(SW(2,0,9))) return(0);    /* store the word */
if (!send_instr(BNE(8,0,-4))) return(0); /* self=-1 */
if (!send_instr(ADDIU(9,9,4))) return(0);   /* next word address */

if (!send_instr(MOVE(31,10))) return(0); /* restore RA */

if (!send_instr(SENDA0)) return(0);	 /* execute it */
instr_buffer_cnt = 0;
PUT_BYTES((Uchar *)vp,n*4);  /* send data */
if (!(readWord().flags&U64_V)) return(0);	 /* wait for done */
current_r8 = current_r9 = mkRV(0);
return(1);
}

/*************************************************************
*  int is_bptReq(Ulong addr,Ulong v) DEF
*	Changes a write_target into a setbp or clrbp
*/
is_bptReq(addr,v)
Ulong addr,v;
{
int n;

#ifdef PMCC
if (!(is_xvwmode() || is_gdbmode())) return(0);

if ((is_xvwmode() && v == XVWBREAKCODE) ||
    (is_gdbmode() && (v == DBXBREAKCODE || v == GDBBREAKCODE))
    ) setbp_target(-1,BPTYPE_PC,addr,0,0);
#else /* dll */
if (breakcode && (v == breakcode)) setbp_target(-1,BPTYPE_PC,addr,0,0);
#endif
else if ((n=which_bpt(addr,0)) != -1) clrbp_target(n);
else return(0);
printDiag(1,"is_bptReq(%08x,%08x)\n",addr,v);
return(1);
}

/*************************************************************
*  int write_target(int type,Ulong addr,U64 v,int sz) DEF
*/
int write_target(int type,Ulong addr,U64 v,int sz)
{
int reg;

printDiag(1,"write_target(%s,%08x,%s,%d)\n", xtcodes[type],addr,
	rvtostr(v),sz);

reg = addr;
switch (type) {
	case XT_PC :
	   switch (reg) {
		case 0 :
		   if (!getSAP()) return(0);
		   write_target(XT_MEM,(Ulong)(ICE_EPC+savearea.sap),v,4);
		   break;
		case 1 :
			if (!writeGpr(8,v)) return(0);
			if (!send_instr(OP_MTHI|(8<<21))) return(0);
			break;
		case 2 :
			if (!writeGpr(8,v)) return(0);
			if (!send_instr(OP_MTLO|(8<<21))) return(0);
			break;
		}
		break; /* 990319 */
	case XT_MEM :
		if (!savearea.sap) getSAP(); /* 980818 */
		/* Be sure that sap info is known before starting to
		 * fill the buffer. Otherwise download can fail.
		 */
#if 0 /* 990324 */
		if (is_cacheable(addr)) {
			dstate = dirty;
			}
		else {
			if (has_wb_dcache && dstate==dirty) {
				flush_target(DCACHE);
				dstate = clean;
				}
			dstate = stale;
			}
#endif
		switch (sz) {
			case 1 : write_target_byte(addr,v.lo); break;
			case 2 : /* 980704 This is a hack */
				if (!send_buffer()) return(0);
				if (!send_bhw(addr,v.lo,2)) return(0);
				break;
			case 4 : 
				if (is_bptReq(addr,v.lo)) break;

				if (lendian_target) swap32n((Uchar *)&v,1);
				if (!write_target_byte(addr,v.lo>>24)) 
					return(0);
				if (!write_target_byte(addr+1,v.lo>>16)) 
					return(0);
				if (!write_target_byte(addr+2,v.lo>>8)) 
					return(0);
				if (!write_target_byte(addr+3,v.lo)) 
					return(0);
				break;
			case 8 : 
				if (lendian_target) swap32n((Uchar *)&v,1);
				if (!write_target_byte(addr,v.hi>>24)) 
					return(0);
				if (!write_target_byte(addr+1,v.hi>>16)) 
					return(0);
				if (!write_target_byte(addr+2,v.hi>>8)) 
					return(0);
				if (!write_target_byte(addr+3,v.hi)) 
					return(0);
				if (!write_target_byte(addr+4,v.lo>>24)) 
					return(0);
				if (!write_target_byte(addr+5,v.lo>>16)) 
					return(0);
				if (!write_target_byte(addr+6,v.lo>>8)) 
					return(0);
				if (!write_target_byte(addr+7,v.lo)) 
					return(0);
				break;
			}
		break;
	case XT_GPR : 
		if (!writeUgpr(reg,v)) return(0);
		break;
	case XT_CP0 : 
		if (!writeGpr(8,v)) return(0);
		if (!send_instr(MTC0(8,reg))) return(0);
		if (!(readA0().flags&U64_V)) return(0);
		break;
	case XT_CP1 :
		if (!writeGpr(8,v)) return(0);
		if (!send_instr(OP_MTC1|(8<<16)|(reg<<11))) return(0);
		if (!(readA0().flags&U64_V)) return(0);
		break;
	case XT_CP1C :
		if (!writeGpr(8,v)) return(0);
		if (!send_instr(OP_CTC1|(8<<16)|(reg<<11))) return(0);
		if (!(readA0().flags&U64_V)) return(0);
		break;
	case XT_DBX :
		if (!writeGpr(8,v)) return(0);
		if (!send_instr(MTD(8,reg))) return(0);
		if (!(readA0().flags&U64_V)) return(0);
		break;
	case XT_CP2 :
	case XT_CP2C :
	case XT_CP3 : 
	case XT_CP3C : 
	default : 
		printDiag(0,"%08x: write_target: bad type\n",type);
		return(0);
	}
return(1);
}

/*************************************************************
*  void freeOcmRec(OcmRec *p) DEF
*	add the OcmRec to freeOcmChain
*	I do this because the IMON interface in crt3.s doesn't
*	  support free();
*/
void freeOcmRec(OcmRec *p)
{
#ifdef PMCC
p->next = freeOcmChain;
freeOcmChain = p;
#else
free(p);
#endif
}

/*************************************************************
*  OcmRec *mallocOcmRec(void) DEF
*	Try to get an OcmRec from freeOcmChain. else use malloc.
*/
OcmRec *mallocOcmRec(void)
{
#ifdef PMCC
OcmRec *p;

if (freeOcmChain) {
	p = freeOcmChain;
	freeOcmChain = p->next;
	return(p);
	}
#endif
return (OcmRec *)malloc(sizeof(OcmRec));
}

/*************************************************************
*  OcmRec *addOcmRec(Ulong addr,Ulong size,Ulong *func,int funclen); DEF
*	arg0 = start address of Ocm region
*	arg1 = size (in bytes) of Ocm region
*	arg2 = optional address of access function. else 0.
*	arg3 = optional length (in words) of access function. else 0.
*/
OcmRec *addOcmRec(Ulong addr,Ulong size,Ulong *func,int funclen)
{
OcmRec *p;

printDiag(1,"addOcmRec(%08x,%08x,%08x,%d)\n",addr,size,func,funclen);
p = mallocOcmRec();
if (!p) {
        printDiag(0,"addOcmRec: malloc failure. Out of memory.\n");
        return(0);
        }
p->addr = addr;
p->size = size;
p->func = func;
p->funclen = funclen;
p->regmap_ok = 0;
p->readonly = 0;
p->map = 0;
p->next = ocmChain;
ocmChain = p;
return(p);
}

/*************************************************************
*  int deleteOcmRec(Ulong addr) DEF
*/
int deleteOcmRec(Ulong addr)
{
OcmRec *p;

printDiag(2,"deleteOcmRec(%08x)\n",addr);
p = ocmChain;
if (!p) return(0);
if (p->addr == addr) { /* 1st item in chain */
	ocmChain = p->next;
	freeOcmRec(p);
	return(1);
	}
for (p=ocmChain;p->next;p=p->next) {
	if (p->next->addr == addr) {
		p->next = p->next->next;
		freeOcmRec(p->next);
		return(1);
		}
	}
return(0);
}

/*************************************************************
*  OcmRec *is_ocm(Ulong addr) DEF
*       return addr of OcmRec if addr falls within an ocm region.
*/
OcmRec *is_ocm(addr)
Ulong addr;
{
OcmRec *p;

for (p=ocmChain;p;p=p->next) {
        if (addr >= p->addr && addr < p->addr+p->size) {
		printDiag(2,"%08x=ocm\n",addr);
		return(p);
		}
        }
return(0);
}

/*************************************************************
*  U64 run_ocm(OcmRec *ocm,int mode,Ulong addr,int sz,Ulong value); DEF
*	Run the designated ocm routine.
*	mode: 0=read 1=write
*/
U64 run_ocm(OcmRec *ocm,int mode,Ulong addr,int sz,Ulong value)
{
int i;
Ulong need;
U64 saved[32];
U64 r,err;

printDiag(1,"run_ocm(x,%s,%08x,%d,%08x)\n",(mode)?"write":"read",addr,sz,value);

if (mode_64bit) err.flags = U64_64;
else err.flags = 0;

/* Can't do this if there's no function */
if (ocm->func == 0 || ocm->funclen == 0) {
	printDiag(1,"run_ocm: ERROR: func=%08x funclen=%d\n",
		ocm->func,ocm->funclen);
	return(err);
	}

/* return if this is a write and ocm region is readonly */
if (mode == 1 && ocm->readonly) {
	printDiag(1,"run_ocm: ERROR: write to ro ocm\n");
	return(err);
	}

/* must know regmap and ibs of kernel */
if (savearea.map == 0) savearea.map = kernel_map;
if (savearea.ibs == 0) savearea.ibs = kernel_ibs;
if (savearea.map == 0 || savearea.ibs == 0) {
	printDiag(1,"run_ocm: ERROR: map=%08x ibs=%d\n",
		savearea.map,savearea.ibs);
	return(err);
	}

if (ocm->funclen > ((int)savearea.ibs)) {
	printDiag(1,"ocm %08x func too large for ibuf.\n",addr);
	return(err);
	}

if (!ocm->regmap_ok) {
	if (!send_buffer()) return(err);
	if (ocm->map == 0) {
		if (!writeGpri(4,2)) return(err); /* regmap function */
		for (i=0;i<ocm->funclen;i++) send_instr(ocm->func[i]);
		ocm->map = readA0().lo;
		}
	printDiag(1,"ocm %08x samap %08x ocmmap %08x.\n",
				addr,savearea.map,ocm->map);
	/* savearea.map is what the kernel saves */
	/* ocm.map is what the ocm routine needs */
	need = (~(savearea.map&ocm->map)) & ocm->map;
	/* need are those extra registers that must be saved */
	if (need) {
		printDiag(1,"ocm %08x need %08x\n",addr,need);
		send_buffer();
		for (i=0;i<32;i++) {
			if (need&(1<<i)) {
				send_instr(OP_OR|(4<<11)|(0<<16)|(i<<21));
				saved[i] = readA0();
				printDiag(1,"ocm %08x save $%d %08x\n",
					addr,i,saved[i]);
				}
			}
		}
	if (!need) ocm->regmap_ok = 1;
	/* regmap_ok indicates that no extra saves are necessary */
	}

if (!send_buffer()) return(err);
if (!writeGpri(4,mode)) return(err);
if (!writeGpri(5,addr)) return(err);
if (!writeGpri(6,sz)) return(err);
if (!writeGpri(7,value)) return(err);
for (i=0;i<ocm->funclen;i++) if (!send_instr(ocm->func[i])) return(err);
r = readA0();
/* restore extra regs */
if (need) {
	for (i=0;i<32;i++) if (need&(1<<i)) {
		printDiag(1,"ocm %08x restore $%d %08x\n",
			addr,i,saved[i]);
		if (!writeGpr(i,saved[i])) return(err);
		}
	if (readA0().flags&U64_V) return(err);
	}
return(r);
}

/*************************************************************
*  Ulong domt1(Ulong sadr,Ulong eadr) DEF
*	Do Memory test part1
*	If it fails, it returns the address of the first failing 
*	location; else zero.
*/
Ulong domt1(Ulong sadr,Ulong eadr)
{
U64 v;

if (!send_buffer()) return(1);
/* part1: walking ones */
if (!writeGpri(4,sadr)) return(1);
if (!writeGpri(9,eadr)) return(1);
/* 1: */
if (!writeGpri(26,1)) return(1);
/* 2: */
if (!send_instr(SW(26,0,4))) return(1);
if (!send_instr(LW(27,0,4))) return(1);
if (!send_instr(0)) return(1);
if (!send_instr(BNE(26,27,6))) return(1); /* 1f */
if (!send_instr(0)) return(1);
if (!send_instr(SLLV(26,1))) return(1);
if (!send_instr(BNE(26,0,-7))) return(1); /* 2b */
if (!send_instr(0)) return(1);
if (!send_instr(ADDIU(4,4,4))) return(1);
if (!send_instr(BNE(4,9,-11))) return(1); /* 1b */
/* 1: */
v = readA0();
if (v.flags&U64_V) return(v.lo);
return(1);
}

/*************************************************************
*  int domt2(Ulong sadr,Ulong eadr) DEF
*	Do Memory test part2
* 	part2: write address to each word
*/
int domt2(Ulong sadr,Ulong eadr)
{
if (!send_buffer()) return(0);
if (!writeGpri(4,sadr)) return(0);
if (!writeGpri(9,eadr)) return(0);
/* 1: */
if (!send_instr(SW(4,0,4))) return(0);
if (!send_instr(ADDIU(4,4,4))) return(0);
if (!send_instr(BNE(4,9,-3))) return(0); /* 1b */
if (readA0().flags&U64_V) return(1);
return(0);
}

/*************************************************************
*  Ulong domt3(Ulong sadr,Ulong eadr) DEF
*	Do Memory test part3
* 	part3: check each word contains address
*	If it fails, it returns the address of the first failing 
*	location; else zero.
*/
Ulong domt3(Ulong sadr,Ulong eadr)
{
U64 v;

if (!send_buffer()) return(1);
if (!writeGpri(4,sadr)) return(1);
if (!writeGpri(9,eadr)) return(1);
/* 1: */
if (!send_instr(LW(26,0,4))) return(1);
if (!send_instr(0)) return(1);
if (!send_instr(BNE(26,4,3))) return(1); /* 1f */
if (!send_instr(0)) return(1);
if (!send_instr(ADDIU(4,4,4))) return(1);
if (!send_instr(BNE(4,9,-6))) return(1); /* 1b */
/* 1: */
v = readA0();
if (v.flags&U64_V) return(v.lo);
return(1);
}