/*************************************************************
 * File: 401x.c
 * Purpose: provide 401x-specific routines for SerialICE drivers
 * Author: Phil Bunce (pjb@carmel.com)
 * Revision History:
 *	970218	Created using code from d4010.c
 *	970218	Tested icache and dcache display commands.
 *	970219	Added code for ilock. Not working yet.
 *	970226	Gated printfs with verbose flag.
 *	970305	wrbpt cmd works - sets ilock bpt.
 *	970305	this doesn't look right - dsoftflush restore
 *	970310	moved brkInstall and brkRemove here from iceif.c and
 *		rewrote them.
 *	970310	Created ilockReq, hwibReq, and hwdbReq.
 *	970311	Removed write_target from method_ram
 *	970312	Merged icache and dcache cmds.
 *	980312	Switched to unified scheme for dll and imon.
 *	980323	Ignore flush_cache requests if target is running.
 *	980421	Added cache_size to cache_cmd
 *	980803	Added isset=0 to brkRemove
 *	980812	Added return if cache_size == 0. Also setFlushneeded().
 *	981222	Changed dwriteback to use dcache_size and cache_line_size.
 */

/*-----------------------------------------------------------------
int flush_target_401x(int mode)
Ulong readCache_401x(int set,int what,Ulong addr)
static void writeCache(int set,int what,Ulong addr,Ulong tag)
void setupcacheline_401x(Ulong addr)
void wrwdtocache_401x(Ulong addr,Ulong val)
int ilockReq_401x(Ulong addr)
void setFlushneeded_401x(Ulong addr,int size)
void brkInstall_401x(int type)
int brkRemove_401x(Ulong epc)
----------------------------------------------------------------*/

#ifndef LR4010
#define LR4010
#endif
#include <imon.h>
#include <mips.h>
#include "iceif.h"

#define ONLY_FLUSH_WHEN_NEEDED /* optimize the flushes */

#define OP_FLUSH_ICACHE	0xbc010000
#define OP_FLUSH_DCACHE	0xbc020000
#define WB_DCACHE(a)	(0xbc040000|((a)<<21))
#define VALID_BIT (1<<1)


int ilock_active;

/*************************************************************
*  int flush_target_401x(int mode)
*	Flush the designated cache in the target.
*/
int flush_target_401x(int mode)
{
Ulong ccc,tmpcfg;
U64 rv;

/* emit code to flush the caches */
/* we are already in an isr with ints disabled */
/* the instr buffer is in kseg1 */

if (!target_stopped) return(1); /* 980323 */

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

switch (mode) {
    case ICACHEI :
	printDiag(1,"hard iflush 401x\n");
	if (icache_size == 0) {
		iflush_needed = 0;
		return(1);
		}
	if (!send_buffer()) return(0);
	/* flush */
	if (!send_instr(OP_FLUSH_ICACHE)) return(0);
	if (!(readA0().flags&U64_V)) return(0);
	iflush_needed = 0;
	break;
    case ICACHE :
#ifdef ONLY_FLUSH_WHEN_NEEDED
	if (!iflush_needed) return(1);
#endif
	printDiag(1,"soft iflush 401x\n");
	if (icache_size == 0) {
		iflush_needed = 0;
		return(1);
		}
	if (!send_buffer()) return(0);
	/* we don't want to flush any sets that have been switched
	* to scratchpad mode.
	*/
	if(!((rv = read_target(XT_CP0,C0_CCC,0)).flags&U64_V)) return(0);
	tmpcfg = ccc = rv.lo;
	if (ccc&CCC_ISR1) {
		printf("skipping is1 flush\n");
		/* isr1 is set, so clear ie1 */
		tmpcfg &= ~CCC_IE1;
		write_target(XT_CP0,C0_CCC,mkRV(tmpcfg),0);
		send_buffer();
		}
	/* flush */
	if (!send_instr(OP_FLUSH_ICACHE)) return(0);
	if (!(readA0().flags&U64_V)) return(0);
	if (!write_target(XT_CP0,C0_CCC,mkRV(ccc),0)) return(0);
	if (!send_buffer()) return(0);
	iflush_needed = 0;
	break;
    case DCACHEI :
	printDiag(1,"hard dflush 401x\n");
	if (dcache_size == 0) {
		dflush_needed = 0;
		return(1);
		}
	if (!send_buffer()) return(0);
	if (!send_instr(OP_FLUSH_DCACHE)) return(0);
	if (!(readA0().flags&U64_V)) return(0);
	dflush_needed = 0;
	break;
    case DCACHE :
#ifdef ONLY_FLUSH_WHEN_NEEDED
	if (!dflush_needed) return(1);
#endif
	printDiag(1,"soft dflush 401x\n");
	if (dcache_size == 0) {
		dflush_needed = 0;
		return(1);
		}
	if (!send_buffer()) return(0);
	/* we don't want to flush any sets that have been switched
	* to scratchpad mode.
	*/

	/* actual cache code */
	/* first we want to write-back the Dcache */
	if (!writeGpri(9,K0BASE)) return(0);
	if (!writeGpri(8,K0BASE+dcache_size)) return(0);
	/* flush loop */
	if (!send_instr(WB_DCACHE(9))) return(0); /* writes 8 words */
	if (!send_instr(ADDIU(9,9,dcache_line_size))) return(0);
	if (!send_instr(BNE(8,9,-3))) return(0);
	if (!(readA0().flags&U64_V)) return(0);
	/* now flush the Dcache */
	if (!send_instr(OP_FLUSH_DCACHE)) return(0);
	if (!(readA0().flags&U64_V)) return(0);

	dflush_needed = 0;
	break;
	}
return(1);
}

/*************************************************************
*  Ulong readCache_401x(int set,int what,Ulong addr)
*	read one word from a cache
*	what: ICACHETAG DCACHETAG ICACHERAM DCACHERAM
*	Note that *all* LDs and STs go to the cache when ISC is set.
*/
Ulong readCache_401x(int set,int what,Ulong addr)
{
Ulong ccc,tmpcfg,tag;

/* we are already in an isr with ints disabled */
/* the instr buffer is in kseg1 */
tmpcfg = ccc = read_target(XT_CP0,C0_CCC,0).lo;

tmpcfg &= ~(CCC_IE1|CCC_IE0|CCC_DE0|CCC_DE1);
tmpcfg |= CCC_ISC;

if (what == ICACHETAG || what == DCACHETAG) tmpcfg |= CCC_TAG;

if (what == ICACHETAG || what == ICACHERAM) {
	if (set) tmpcfg |= CCC_IE1;
	else tmpcfg |= CCC_IE0;
	}
else if (what == DCACHETAG || what == DCACHERAM) {
	if (set) tmpcfg |= CCC_DE1;
	else tmpcfg |= CCC_DE0;
	}

writeGpri(9,addr);
writeGpri(8,tmpcfg);
send_instr(MTC0(8,C0_CCC)); /* 3 nops before ld or st */
send_instr(0);
send_instr(0);
send_instr(0);

/* read the cache */
send_instr(LW(4,0,9));
send_instr(0);

/* restore CCC */
writeGpri(8,ccc);
send_instr(MTC0(8,C0_CCC));
tag = readA0().lo;

return(tag);
}

/*************************************************************
*  static void writeCache(int set,int what,Ulong addr,Ulong tag)
*	UNTESTED
*/
static void writeCache(int set,int what,Ulong addr,Ulong tag)
{
Ulong ccc,tmpcfg;

/* we are already in an isr with ints disabled */
/* the instr buffer is in kseg1 */
tmpcfg = ccc = read_target(XT_CP0,C0_CCC,0).lo;

tmpcfg &= ~(CCC_IE1|CCC_IE0|CCC_DE0|CCC_DE1);
tmpcfg |= CCC_ISC;

if (what == ICACHETAG || what == DCACHETAG) tmpcfg |= CCC_TAG;

if (what == ICACHETAG || what == ICACHERAM) {
	if (set) tmpcfg |= CCC_IE1;
	else tmpcfg |= CCC_IE0;
	}
else if (what == DCACHETAG || what == DCACHERAM) {
	if (set) tmpcfg |= CCC_DE1;
	else tmpcfg |= CCC_DE0;
	}

#if 0 /* old */
write_target(XT_CP0,C0_CCC,tmpcfg,0);
send_buffer();

write_target(XT_MEM,addr,tag,4);

/* restore cfg reg */
write_target(XT_CP0,C0_CCC,ccc,0);
send_buffer();

#else /* new */
writeGpri(8,tmpcfg);
send_instr(MTC0(8,C0_CCC)); /* 3 nops before ld or st */
send_instr(0);
send_instr(0);
send_instr(0);

/* write the cache */
writeGpri(8,tag);
writeGpri(9,addr);
send_instr(SW(8,0,9));
send_instr(0);

/* restore CCC */
writeGpri(8,ccc);
send_instr(MTC0(8,C0_CCC));
readA0();
#endif

}

#ifdef PMCC
/*************************************************************
*/
Optdesc cache_opts_401x[] = {
	{"[-idv][set [addr]]","display cache"},
	{"set","select set (default 0)"},
	{"addr","specify address"},
	{"-i","display icache"},
	{"-d","display dcache (default)"},
	{"-v","display only valid entries"},
	{0}};

int cache_cmd_401x(int ac,char *av[])
{
int n,i,j,set,vflag,iflag;
int cacheram,cachetag,cache_size,cache_line_size;
static Ulong next_adr;
Ulong adr,tag,ccc;
U64 rv;

vflag = iflag = 0;
for (n=0,i=1;i<ac;i++) {
	if (av[i][0] == '-') {
		if (strequ(av[i],"-v")) vflag = 1;
		else if (strequ(av[i],"-i")) iflag = 1;
		else if (strequ(av[i],"-d")) iflag = 0;
		else {
			printf("%s: unknown option\n",av[i]);
			return;
			}
		}
	else if (n==0) {
		if (!get_rsa(&rv,av[i])) return;
		set = rv.lo;
		n++;
		}
	else if (n==1) {
		if (!get_rsa(&rv,av[i])) return;
		adr = rv.lo;
		n++;
		}
	else {
		printf("%s: too many args\n",av[i]);
		return;
		}
	}
if (n==0) set = 0;
if (n<2) adr = next_adr;

if (iflag) {
	cachetag = ICACHETAG;
	cacheram = ICACHERAM;
	cache_size = icache_size;
	cache_line_size = icache_line_size;
	}
else {
	cachetag = DCACHETAG;
	cacheram = DCACHERAM;
	cache_size = dcache_size;
	cache_line_size = dcache_line_size;
	}

/* cache lines always start on 32-byte boundaries */
adr &= ~(cache_line_size-1); 

printf("Displaying %s set %d\n",(iflag)?"icache":"dcache",set);

ccc = read_target(XT_CP0,C0_CCC,0).lo;
if (    (iflag && set == 1 && ccc&CCC_ISR1) ||
	(!iflag && set == 0 && ccc&CCC_SR0) ||
	(!iflag && set == 1 && ccc&CCC_SR1))
		printf("This set is in scatchpad mode\n");

for (n=j=0;j<8;adr += cache_line_size,n++) {
	if (n > cache_size/cache_line_size) break;
	tag = readCache_401x(set,cachetag,adr);
	if (vflag && (tag&VALID_BIT)==0) continue;
	printf("%08x  %08x ",adr,readCache_401x(set,cacheram,adr));
	printf("%08x ",readCache_401x(set,cacheram,adr+4));
	printf("%08x ",readCache_401x(set,cacheram,adr+8));
	printf("%08x\n          ",readCache_401x(set,cacheram,adr+12));
	printf("%08x ",readCache_401x(set,cacheram,adr+16));
	printf("%08x ",readCache_401x(set,cacheram,adr+20));
	printf("%08x ",readCache_401x(set,cacheram,adr+24));
	printf("%08x ",readCache_401x(set,cacheram,adr+28));
	printf(" %08x\n",tag);
	j++;
	}
next_adr = adr;
}
#endif

/*************************************************************
*  void setupcacheline_401x(addr)
*	Switch the icache set1 to scratchpad mode.
*	Copy real memory bytes to the correct line.
*	Write the correct tag and valid bit.
*	Exit with ISR1 set.
*/
void setupcacheline_401x(Ulong addr)
{
Ulong ccc,tmpcfg,addrmsk;
int n;

ccc = read_target(XT_CP0,C0_CCC,0).lo;
ccc |= (CCC_IE1|CCC_IS8);
tmpcfg = ccc;
tmpcfg &= ~(CCC_IE0|CCC_DE0|CCC_DE1|CCC_IE1);

/* copy data from memory */
writeGpri(8,addr&~0x1f);
writeGpri(9,K1BASE|addr&~0x1f);
writeGpri(10,8);

n = 1; /* start of loop */
n += writeGpri(4,ccc);
n += send_instr(MTC0(4,C0_CCC)); /* 3 nops before ld or st */
n += send_instr(0);
n += send_instr(0);
n += send_instr(0);
n += send_instr(LW(2,0,9));

n += writeGpri(4,tmpcfg|CCC_ISC|CCC_IE1);
n += send_instr(MTC0(4,C0_CCC)); /* 3 nops before ld or st */
n += send_instr(0);
n += send_instr(ADDIU(9,9,4));
n += send_instr(SUBIU(10,1));
n += send_instr(SW(2,0,8));
n += send_instr(BNE(10,0,0-n));
send_instr(ADDIU(8,8,4));
/* end of loop */

/* write the tag */
writeGpri(8,tmpcfg|CCC_TAG|CCC_ISC|CCC_IE1);
send_instr(MTC0(8,C0_CCC)); /* 3 nops before ld or st */
send_instr(0);
writeGpri(9,addr);
addrmsk = (0x7<<29)|((8*1024)-1);
writeGpri(8,(addr&~addrmsk)|VALID_BIT);
send_instr(SW(8,0,9));

/* restore CCC */
writeGpri(8,ccc|CCC_ISR1|CCC_IE1);
send_instr(MTC0(8,C0_CCC)); /* 3 nops before ld or st */
send_instr(0);
send_instr(0);
readA0();
}

/*************************************************************
*  void wrwdtocache_401x(addr,val)
*	Writes one word to the iscratchpad. 
*	Assumes that setupcacheline() has already been called.
*/
void wrwdtocache_401x(Ulong addr,Ulong val)
{
Ulong ccc,tmpcfg;

ccc = read_target(XT_CP0,C0_CCC,0).lo;
tmpcfg = ccc;
tmpcfg &= ~(CCC_IE0|CCC_DE0|CCC_DE1|CCC_ISR1);
tmpcfg |= CCC_ISC;
writeGpri(8,tmpcfg);
send_instr(MTC0(8,C0_CCC)); /* 3 nops before ld or st */
send_instr(0);

writeGpri(8,val);
writeGpri(9,addr);
send_instr(SW(8,0,9));

/* restore CCC */
writeGpri(8,ccc);
send_instr(MTC0(8,C0_CCC)); /* 3 nops before ld or st */
send_instr(0);
send_instr(0);
readA0();
}

/*************************************************************
*  int ilockReq_401x(addr)
*	verify that other ilock bpts don't conflict with this one
*		(ie. are a cachesize multiple apart).
*	Also verify that ISR1 bit is not already set.
*/
int ilockReq_401x(Ulong addr)
{
Ulong tmsk,omsk;
int i;

/* return error if instr scratchpad ram is in use */
if (read_target(XT_CP0,C0_CCC,0).lo&CCC_ISR1) return(0); 

tmsk = (icache_size)-1; /* tag mask */
omsk = tmsk&~(16-1); /* offset mask: line size 16B */
for (i=0;i<MAX_BPT;i++) {
	if (brkList[i].type==0) continue;
	if (brkList[i].method != BRK_METHOD_ROM) continue;
	if ((addr&omsk) != (brkList[i].addr&omsk)) continue; /* not same line */
	if ((addr&~tmsk) == (brkList[i].addr&~tmsk)) return(0); /* same tag */
	}
return(1);
}

/*************************************************************
*/
void setFlushneeded_401x(Ulong addr,int size)
{
if (is_ocm(addr)) ; /* 980225 */
else {
	printDiag(1,"iflush & dflush needed\n");
	if (icache_size != 0) iflush_needed = 1;
	if (dcache_size != 0) dflush_needed = 1;
	}
}


/*************************************************************
*  void brkInstall_401x(type)
*	type=1 install regular+temp bpts
*	type=2 install trace bpts
*/
void brkInstall_401x(int type)
{
int i,flag;
Ulong dcs,addr;
OcmRec *p;

printDiag(1,"brkInstall_401x(%d)\n",type);
flag = 0;
send_buffer();
for (i=0;i<MAX_BPT;i++) {
	/* first discard the entries we aren't going to handle */
	if (brkList[i].type==0) continue;
	if (type == 1 && brkList[i].type == BPTYPE_TRACE) continue;
	if (type == 2 && brkList[i].type != BPTYPE_TRACE) continue;

	addr = brkList[i].addr;
	switch (brkList[i].method) {
	    case BRK_METHOD_RAM :
		printDiag(1,"installing ram bpt at %08x\n",addr);
		if ((p=is_ocm(addr)) && p->func) {
			brkList[i].val = run_ocm(p,0,addr,4,0).lo;
			run_ocm(p,1,addr,4,BPT_CODE);
			}
		else {
			brkList[i].val = read_target32(addr);
			/* write_target(XT_MEM,addr,BPT_CODE,4); */
			printDiag(1,"sw %08x->%08x\n",BPT_CODE,addr);
			writeGpri(8,BPT_CODE);
			writeGpri(9,addr);
			send_instr(SW(8,0,9));
			readA0().lo;
			setFlushneeded(addr,4);
			}
		brkList[i].isset = 1;
		break;

	    case BRK_METHOD_ROM :
		printDiag(1,"installing rom bpt at %08x\n",addr);
		if (!flag) flush_target(ICACHE);
		flag = 1;
		if (!(readCache_401x(1,ICACHETAG,addr)&VALID_BIT))
			setupcacheline_401x(addr);
		wrwdtocache_401x(addr,BPT_CODE);
		brkList[i].isset = 1;
		break;

	    case BRK_METHOD_HW :
		if (brkList[i].type == BPTYPE_DATA) {
			printDiag(1,"installing hwdb bpt at %08x\n",addr);
			writeGpri(8,addr);
			send_instr(MTC0(8,DBX_BDA));
			writeGpri(8,brkList[i].mask);
			send_instr(MTC0(8,DBX_BDAM));
			send_instr(0);
			send_instr(MFC0(4,DBX_DCS));
			send_instr(0);
			dcs = readA0().lo;
			dcs |= DCS_TR|DCS_UD|DCS_KD|DCS_DE|DCS_DAE;
			if (brkList[i].aux[0]&2) dcs |= DCS_DR;
			if (brkList[i].aux[0]&1) dcs |= DCS_DW;
			}
		else {
			printDiag(1,"installing hwib bpt at %08x\n",addr);
			writeGpri(8,addr);
			send_instr(MTC0(8,DBX_BPC));
			writeGpri(8,brkList[i].mask);
			send_instr(MTC0(8,DBX_BPCM));
			send_instr(0);
			send_instr(MFC0(4,DBX_DCS));
			send_instr(0);
			dcs = readA0().lo;
			dcs |= DCS_TR|DCS_UD|DCS_KD|DCS_DE|DCS_PCE;
			}
		writeGpri(8,dcs);
		send_instr(MTC0(8,DBX_DCS));
		readA0();
		brkList[i].isset = 1;
		break;
	    default : 
		printDiag(0,"%d: error bad method\n",brkList[i].method);
		return;
	    }
	}
}

/*************************************************************
*  int brkRemove_401x(epc)
*	returns type: 0=none 1=bpc 2=bda 3=itemp 4=sstep
*/
int brkRemove_401x(Ulong epc)
{
int i,type,flag;
Ulong ccc;
OcmRec *p;

printDiag(1,"brkRemove_401x(%08x)\n",epc);
send_buffer();
type = flag = 0;
for (i=0;i<MAX_BPT;i++) {
	/* first discard the entries we aren't going to handle */
	if (brkList[i].type==0) continue;
	if (brkList[i].isset==0) continue;

	if (epc == brkList[i].addr) type = brkList[i].type;
	switch (brkList[i].method) {
	    case BRK_METHOD_RAM :
		if ((p=is_ocm(brkList[i].addr)) && p->func) {
			run_ocm(p,1,brkList[i].addr,4,brkList[i].val);
			}
		else {
			printDiag(1,"sw %08x->%08x\n",
				brkList[i].val,brkList[i].addr);
			writeGpri(8,brkList[i].val);
			writeGpri(9,brkList[i].addr);
			send_instr(SW(8,0,9));
			readA0();
			setFlushneeded(brkList[i].addr,4);
			}
		brkList[i].isset = 0;
		break;

	    case BRK_METHOD_ROM :
		ccc = read_target(XT_CP0,C0_CCC,0).lo;
		writeGpri(8,ccc&~CCC_ISR1);
		send_instr(MTC0(8,C0_CCC));
		readA0();
		brkList[i].isset = 0;
		setFlushneeded(brkList[i].addr,4);
		break;

	    case BRK_METHOD_HW :
		writeGpri(8,0);
		send_instr(MTC0(8,DBX_DCS));
		readA0();
		brkList[i].isset = 0;
		break;
	    }
	if (brkList[i].type == BPTYPE_ITMP) brkList[i].type = 0;
	if (brkList[i].type == BPTYPE_TRACE) brkList[i].type = 0;
	}
return(type);
}