// Parallel Flash Programming Mode 
#include <REG1210.H>
#include "mvp.h"
SLAVE target;
unsigned char fpm(unsigned char cmd, unsigned int a,  unsigned char pd)
{ unsigned char c,cpu, cmdx,rdcmd;
	// Warning: issues SRD/FRD to both CPUA & CPUS will cause bus conflict!!
	cpu=cmd & 0xf0; cmdx=cmd & 0x0f;		
	if (cmdx== SRD || cmdx==FRD) {rdcmd=1; P0DDR=P0IDLE; id=0xff;} 
	else {P0DDR=P0OUT; id = pd; rdcmd=0;}
	ah= (unsigned char)(a>>8); CMD0 = cmdx &0x01; CP=0; CP=1; CMD1= cmdx &0x02; 
	al=(unsigned char)a; CMD2= cmdx & 0x04;
	if (cpu==CPUA) {	// Aux CPU command
		REQA=1; while(ACKA==0){}; c=(unsigned char)!PASSA;
		if (rdcmd!=0) c=id; else { P0DDR=P0IDLE; id=0xff; }
		REQA=0; while(ACKA==1){};	
	} else if (cpu==CPUS) {  //Slave CPU command
		REQS=1; while(ACKS==0){}; c=(unsigned char)!PASSS;
		if (rdcmd!=0) c=id; else { P0DDR=P0IDLE; id=0xff; }
		REQS=0; while(ACKS==1){};	
	} else if (cpu==CPUBOTH) {  //Slave & Aux CPU command
		REQS=1; REQA=1; while(ACKS==0 || ACKA==0){};
		c=(unsigned char)!(PASSS & PASSA); P0DDR=P0IDLE; id=0xff; 
		REQS=0; REQA=0; while(ACKS==1 || ACKA==1){};	
	}	return(c);
}
unsigned char afpm(unsigned char cmd, unsigned int a,  unsigned char pd)
{ unsigned char c, rdcmd=0;
	ah= (unsigned char)(a>>8); 
	CP=0; CP=1;
	al=(unsigned char)a; 
	if (cmd== SRD || cmd==FRD) rdcmd=1; else {P0DDR=P0OUT; id = pd;}
	CMD0 = (cmd & 1); CMD1= (cmd>>1) &1; CMD2= (cmd>>2) &1;
	REQA=1;
	while(ACKA==0){};
	c=(unsigned char)!PASSA;
	if (rdcmd) c=id; else { P0DDR=P0IDLE; id=0xff; }
	REQA=0;
	while(ACKA==1){};	
	return(c);
}
unsigned char sfpm(unsigned char cmd, unsigned int a,  unsigned char pd)
{ unsigned char rdcmd=0, err=0;
	if (cmd== SRD || cmd==FRD) {
		rdcmd=1;
	} else {P0DDR=P0OUT; id = pd;}
	ah= (unsigned char)(a>>8); 
	CP=0; CP=1;
	al=(unsigned char)a; 
	CMD0 = (cmd & 1); CMD1= (cmd>>1) &1; CMD2= (cmd>>2) &1;
	REQS=1;
	t1=100; while(ACKS==0 && t1!=0){}; if (t1==0) err=1;
	if (PASSS==0) err=1;;
	if (rdcmd) err=id; else { P0DDR=P0IDLE; id=0xff; }
	REQS=0;
	t1=100; while(ACKS==1 && t1!=0){}; if (t1==0) err=1;
	return(err);
}
void led(unsigned char color, unsigned char onoff)
{  unsigned char c;
	c=afpm(SRD,sP3,0);
	if (color==RED && onoff==ON) c &=0xfe; 
	else if (color==RED && onoff==OFF) c |=0x01; 
	else if (color==GREEN && onoff==ON) c &=0xfd;
	else if (color==GREEN && onoff==OFF) c |=0x02; 
	afpm(SWR, sP3,  c);
}
void pasel(unsigned char c)
{  unsigned char k=PASEL;
	if (c==PSEN_PSEN)		{ k&=0xc7; k|=0x00; } if (c==PSEN_CLK)		{ k&=0xc7; k|=0x10; } if (c==PSEN_MODCLK)	{ k&=0xc7; k|=0x20; }
	if (c==PSEN_LOW)		{ k&=0xc7; k|=0x30; } if (c==PSEN_HIGH)	{ k&=0xc7; k|=0x38; } 
	if (c==ALE_ALE)		{ k&=0xfc; k|=0x00; } if (c==ALE_LOW)		{ k&=0xfc; k|=0x02; } if (c==ALE_HIGH)		{ k&=0xfc; k|=0x03; }
	PASEL=k;
}
void release(void)
{	pasel(PSEN_LOW);
	ALE=0; 
	CMD0=0; CMD1=0; CMD2=0; id=0; 
	PASSS=0; PASSA=0; ACKS=0; ACKA=0;
	ah=0; CP=!CP; CP=!CP; al=0; 
	MRST=0; 	REQA=0; REQS=0; 
	VDDEN=0;
}
unsigned char resetpfpm(void)
{	unsigned char err=0;
	unsigned char i;
	MRST=1; REQA=1; VDDEN=1;
	if (SCALE==1) pasel(PSEN_CLK); else pasel(PSEN_PSEN);
	P0DDR=P0IDLE; //P0 GPI 
	P1DDR=P1OUT; //P1 GPO 
	P2DDR=0;
	P3DDR=0;
	PASSS=1; PASSA=1; ACKS=1; ACKA=1;
	al=0; CP=0; CP=1; // AH=AL=0
	CMD2=0; CMD1=0; CMD0=0;
	REQA=1; REQS=1; ALE=1;
	//P2DDR=0x5144; //P2: OOCO OCOC
	//P3DDR=0x5447; //P3: OOOC OCOI
	t0=50; while(t0!=0 && !high(vdds)){;} // wait 100ms or VDDS ready
	t0=50; while(t0!=0 && !(PASSA==1 && PASSS==1 && ACKA==1 && ACKS==1 && ALE==1)){;} // wait 100ms or till CPUA&CPUS ready
	if (PASSA==0 || PASSS==0 || ACKA==0 || ACKS==0 || ALE==0) err=1;
	ALE=0; 			// ALE=0, CPUA & CPUS RST => PFPM
	REQA=0; REQS=0;
	MRST=0;
	t0=100; while(ACKA==1 && t0!=0){}; // wait 200ms or ACKA==0
	if (ACKA==1) err=2;		// No ACKA from CPUA 
	else if (afpm(SRD,sFTCON,0)!=0xA5) err=3; // CPUA SFR read error
	i=vdds; if (!high(i)) err=4;
	if (err!=0) {
		putcr(); putstr("\nFatal:");
		if (err==1) putstr("PASS/ACK/ALE line stuck");
		else if (err==2) putstr("No ACKA from CPUA");
		else if (err==3) putstr("CPUA SFR read error");
		else if (err==4) {putstr("Vdds Short circuit at "); tx_hex(i);}
		else putstr("Unknown Err");
		release();
		return(err);
	}
	afpm(SWR,sP3DDRL,0x55); 
	afpm(SWR,sUSEC,cUSEC);	// CPUA timers
	afpm(SWR,sMSECL,(cMSEC & 0xff)); 
	afpm(SWR,sMSECH,(cMSEC>>8)); 
	t0=100; while(ACKS==1 && t0!=0){}
	if (sfpm(SRD,sFTCON,0)!=0xA5 || ACKS==1) {
		target.exist=-1;
	} else {
		sfpm(SWR,sUSEC,cUSEC);	// CPUS timers
		sfpm(SWR,sMSECL,(cMSEC & 0xff)); 
		sfpm(SWR,sMSECH,(cMSEC>>8)); 
		target.HWPC1=sfpm(SRD,sHWPC1,0);
		target.HWPC0=sfpm(SRD,sHWPC0,0);
		target.HWID=sfpm(SRD,sHWID,0);
		target.secure=((sfpm(FRD,0x807f,0)&0x80)==0)?1:0; 
		if (target.HWPC1==0 && (target.HWPC0&0x80)==0){ // Found MSC1210 
			target.exist=1;
			target.part=MSC1210;
			if ((target.HWPC0 & 0x03)==0) {target.kbyte=4; target.y=2;}  //Y2
			if ((target.HWPC0 & 0x03)==1) {target.kbyte=8; target.y=3;}  //Y3
			if ((target.HWPC0 & 0x03)==2) {target.kbyte=16; target.y=4;}  //Y4
			if ((target.HWPC0 & 0x03)==3) {target.kbyte=32; target.y=5;}  //Y5
		}
		else if (target.HWPC1==0x08 && (target.HWPC0&0x80)==0){ // Found MSC1211/12
			target.exist=1;
			if ((target.HWPC0 &0x04)==0) target.part=MSC1212; else target.part=MSC1211;
			if ((target.HWPC0 & 0x03)==0) {target.kbyte=4; target.y=2;}  //Y2
			if ((target.HWPC0 & 0x03)==1) {target.kbyte=8; target.y=3;}  //Y3
			if ((target.HWPC0 & 0x03)==2) {target.kbyte=16; target.y=4;}  //Y4
			if ((target.HWPC0 & 0x03)==3) {target.kbyte=32; target.y=5;}  //Y5
		} else target.exist=0; // Device Unknown
	}
	return(err);
}
unsigned char blackout(unsigned int i)
{  unsigned char c;
	if ((i>>8)==0x80){ // blackout on hex 80xx
		c=i&0x00ff;
		if (c==0 || c==1 || (c>=0x70 && c<=0x7D)) return(1); else return(0);
	} else return(0);
}
#define Pass		0
#define Fail		1
#define P			Pass
#define F			Fail
#define RETENTION	0xaa
// March Test
#define MarchEnd	255
#define MarchBeg	0  //4k
#define MarchNum	5
#define MarchElement	7
#define UpDown		(MarchElement-1)
char march(unsigned char cpu)
{	long int li=0;
	unsigned char code m[MarchNum][MarchElement]= {	
				'w', 0 , 0 , 0 , 0 , 0 ,'U',
				'r','W','R','w','r','W','U',
 			   'R','w','W', 0 , 0 , 0 ,'U',
				'R','w','W','w', 0 , 0 ,'D',
				'r','W','w', 0 , 0 , 0 ,'D',
	};
	unsigned char mn, mn_done, me, me_done, p,  pg_done, index, index_done;
	unsigned char bi,ei,inc,val,op;
	unsigned char c,result, b,e;
	unsigned int pa;
	unsigned char a, trial=0;
	li++;
	result=Pass;
	mn=0; mn_done=0;
	while (!mn_done){
		tx_byte(mn+'0');
		c=m[mn][UpDown];
		if (c=='U') {b=MarchBeg; e=MarchEnd; inc=1; bi=0; ei=127;}
		else {b=MarchEnd; e=MarchBeg; inc=-1; bi=127; ei=0;}
		p=b; pg_done=0;
		while(!pg_done) {
			resetpfpm();
			pa=p*128; me=0; me_done=0;
			if ((p&0x01)==0x01) led(RED,ON); 
			if ((p&0x01)==0x00) led(RED,OFF);
			while (!me_done) {
				op=m[mn][me];
				if (op==0 || op=='U' || op=='D') {me_done=1;}
				if (op=='w' || op=='r') val=ZERO; else val=ONE;
				if (op=='w' || op=='W') {				 
					c=fpm(cpu|FPE,pa, 0x0ff); 	// erase destination page
					if (c!=0){putstr("ErrFPE: "); result=Fail; pg_done=1; index_done=1; me_done=1; mn_done=1;} else {
						index=bi; index_done=0;
						//putstr("\nER-");tx_hex(pa >> 8); tx_hex(pa); 
					}
					while(!index_done){ 
						c=fpm(cpu|FWR,index+pa,val);
						if (c!=0){
							if (trial>3){
								putstr("ErrFWR: "); result=Fail; pg_done=1; index_done=1; me_done=1; mn_done=1;
							} else trial++;
						} else {
							trial=0;
							if (index==ei) index_done=1;
							index +=inc;
							//tx_byte('.');	
						}
					}
				}
				if (op=='r' || op=='R') {
					index=bi;
					while(!index_done){ 
						c=fpm(cpu|FRD,index+pa,0);
						if (c!=val){result=Fail; pg_done=1; index_done=1; me_done=1; mn_done=1;} else {
							if (index==ei) index_done=1;
							index +=inc;
						}
					}
				}
				me++;
				if(me==MarchElement) me_done=1;
			}
			if (result==Fail){
				tx_byte(result);
				putstr(" MN="); tx_hex(mn);