/* For free support for VSIDE, please visit www.vsdsp-forum.com */

// Starting point template for creating VSOS3 libraries and device drivers.
// This will create a <projectname>.DL3 file, which you can copy to 
// your VS1005 Developer Board's system disk's SYS subdirectory.

// If init(), main() or fini() are not needed, remove them from the solution.
// There's no need to have any unneeded functions in the library.

#include <vo_stdio.h>
#include <volink.h>     // Linker directives like DLLENTRY
#include <apploader.h>  // RunLibraryFunction etc
#include <kernel.h>
#include <string.h>
#include <devSpiFlash.h>

#define RESERVED_SECTORS (2*65536/512) // Reserve 128KB from the beginning of flash for boot code
#define SPI_FLASH_COMMAND_WRITE_ENABLE  0x06
#define SPI_FLASH_COMMAND_WRITE_DISABLE  0x04
#define SPI_FLASH_COMMAND_READ_STATUS_REGISTER  0x05
#define SPI_FLASH_COMMAND_WRITE_STATUS_REGISTER  0x01
#define SPI_FLASH_COMMAND_READ  0x03
#define SPI_FLASH_COMMAND_WRITE 0x02
#define SPI_FLASH_COMMAND_CLEAR_ERROR_FLAGS 0x30
#define SPI_FLASH_COMMAND_ERASE_BLOCK 0xD8
#define SPI_FLASH_COMMAND_ERASE_SECTOR 0x20
#define SPI_FLASH_COMMAND_ERASE_CHIP 0xC7
#define END_FRAME 1
#define DONT_END_FRAME 0



u_int16 verbatim = 0;
DiskGeometry g;
FILE *ifp = NULL;
DEVICE *ph = NULL;
DEVICE *flash = NULL;
u_int16 buf[256];
u_int16 buf2[256];
u_int32 sectors = 0;
u_int32 fileSectors = 0;
u_int16 fileBlocks = 0;
s_int16 firstBlock = 1;
/*
void Backup(void) {
	u_int32 n;
	
	for (n=0; n<128; n++) {
		flash->BlockRead(flash, n-RESERVED_SECTORS, 1, buf);
		fwrite(buf, 256, 1, of); 		
	}
	for (n=128; n<256; n++) {
		if (verbatim) {
			flash->BlockRead(flash, n-RESERVED_SECTORS, 1, buf);
		} else {
			flash->BlockRead(flash, n-(RESERVED_SECTORS + 128), 1, buf);
		}
		fwrite(buf, 256, 1, of); 		
	}
	for (n=256; n<sectors; n++) {
		flash->BlockRead(flash, n-RESERVED_SECTORS, 1, buf);
		fwrite(buf, 256, 1, of); 	
	}	

}
*/

void SingleCycleCommand(register __i0 DEVICE *ph, u_int16 cmd) {
	ph->Ioctl(ph, IOCTL_START_FRAME, 0);
	ph->Write(ph, NULL, cmd, 1);
	ph->Ioctl(ph, IOCTL_END_FRAME, 0);
}

void MultipleCycleCommand(register __i0 DEVICE *ph, u_int16 *cmd, u_int16 cycles, u_int16 endFrame) {
	ph->Ioctl(ph, IOCTL_START_FRAME, 0);
	ph->Write(ph, cmd, 0, cycles);
	if (endFrame) {
		ph->Ioctl(ph, IOCTL_END_FRAME, 0);
	}
}

u_int16 SpiFlashWaitStatus(register __i0 DEVICE *ph) {
	u_int16 status;
	ph->Ioctl(ph, IOCTL_START_FRAME, 0);
	ph->Write(ph, NULL, SPI_FLASH_COMMAND_READ_STATUS_REGISTER, 1);
	do {
		status = ph->Read(ph,0,0,0);
	} while (status & 1);
	ph->Ioctl(ph, IOCTL_END_FRAME, 0);
	return status;
	
}

void FlashUnprotect(register __i0 DEVICE *ph) {
	SingleCycleCommand(ph, SPI_FLASH_COMMAND_WRITE_ENABLE);
	MultipleCycleCommand(ph, "\p\x01\x02", 2, END_FRAME); //Write Status Register (0x01): Sector Protect Off (0x02)
	SpiFlashWaitStatus(ph);
	SingleCycleCommand(ph, SPI_FLASH_COMMAND_WRITE_ENABLE);
}


ioresult Erase64K (register __i0 DEVICE *ph, register u_int32 firstBlock) {
	u_int16 cmd[2];
	u_int16 i,j;
	//printf("ERASE%ld ",firstBlock);	
	SingleCycleCommand(ph, SPI_FLASH_COMMAND_WRITE_ENABLE);
	SingleCycleCommand(ph, SPI_FLASH_COMMAND_CLEAR_ERROR_FLAGS);
	FlashUnprotect(ph);

	cmd[0] = (firstBlock>>7) | (SPI_FLASH_COMMAND_ERASE_BLOCK << 8);
	cmd[1] = (firstBlock<<9);	
	MultipleCycleCommand(ph, cmd, 4, END_FRAME);
	SpiFlashWaitStatus(ph);
	for (i=0; i<128; i++) {
		flash->BlockRead(flash, firstBlock+i-RESERVED_SECTORS, 1, buf);
		for (j=0; j<256; j++) {
			if (buf[j] != 0xffff) {
				printf("Erase error\n",i);
				return S_ERROR;
				break;
			}
		}
	}
	return S_OK;
}

void ProgramSector(register __i0 DEVICE *ph, register u_int32 firstBlock, u_int16 *data) {
	u_int16 cmd[2];	
	u_int16 i;
	//printf("PROGRAM%ld ",firstBlock);	
	FlashUnprotect(ph);
	cmd[0] = (firstBlock>>7) | (SPI_FLASH_COMMAND_WRITE << 8);
	cmd[1] = (firstBlock<<9); //first 256 bytes of block
	MultipleCycleCommand(ph, cmd, 4, DONT_END_FRAME);
	ph->Write(ph, data, 0, 256);
	ph->Ioctl(ph, IOCTL_END_FRAME, 0);
	SpiFlashWaitStatus(ph);
					
	FlashUnprotect(ph);
	cmd[1] |= (1 << 8); //"higher" 256 bytes of block
	MultipleCycleCommand(ph, cmd, 4, DONT_END_FRAME);
	ph->Write(ph, data, 256, 256); //Write 256 bytes from position 256 bytes
	ph->Ioctl(ph, IOCTL_END_FRAME, 0);
	SpiFlashWaitStatus(ph);
}

void Restore(void) {
	s_int16 b;
	u_int32 s;
	u_int16 i;
	u_int16 j;
	
	printf("Reprogramming flash from end to beginning.\n");

	//for (b=firstBlock; b<fileBlocks; b++) {
	for (b=fileBlocks-1; b>=firstBlock; b--) {
		s = (u_int32)b * 128;	
		printf("Block %2d (%dK..%dK) Erase.. ",b,(b*64),(b*64)+63);		
		if (Erase64K(ph,s)) {
			return;
		}
		printf("Write.. ");
		ifp->pos = s*512;
		for (i=0; i<128; i++) {
			memset(buf, 0xffff, 256);
			if (!feof(ifp)) {
				fread(buf, 256, 1, ifp);
			}
			ProgramSector(ph, s, buf); 	
			flash->BlockRead(flash, s-RESERVED_SECTORS, 1, buf2);
			for (j=0; j<256; j++) {
				if (buf[j] != buf2[j]) {
					//printf("%04x!%04x ",buf2[j],buf[j]);
					printf("Programming error\n");
					return;
				}
			}
			s++;	
		}
		printf("\n");	

	}
}


u_int16 Verify(void) {
	u_int16 b;
	u_int32 s;
	u_int16 i;
	u_int16 j;
	u_int16 ok;
	u_int16 all_ok = 1;
	
	printf("Verifying written data..\n");
	for (b=firstBlock; b<fileBlocks; b++) {
		s = (u_int32)b * 128;	
		printf("Block %2d (%dK..%dK) ",b,(b*64),(b*64)+63);		
		ifp->pos = s*512;
		ok = 1;
		for (i=0; i<128; i++) {
			fread(buf, 256, 1, ifp);			
			flash->BlockRead(flash, s-RESERVED_SECTORS, 1, buf2);
			for (j=0; j<256; j++) {
				if (buf[j] != buf2[j]) {
					//printf("%04x!%04x ",buf2[j],buf[j]);
					ok=0;
					all_ok = 0;
				}
			}
			s++;
		}
		printf(ok ? "OK" : "MALPROGRAMMED");
		printf("\n");	
	}
	return all_ok;
}

// Startup code for each instance of the library
// If CONFIG.TXT has several instance of the same driver, this is called for each line.
ioresult main(char *parameters) {
	int nParam = RunProgram("ParamSpl",parameters);
	char *p = parameters;
	int i;
	int retries = 0;
	int all_ok = 0;
	

	if (nParam == 0) {
		printf("Usage: sysrestr [-a] x:filename.bin\n");
		printf("Parameters:\n");
		printf("  -a  Restore 1:1 backup. (Default: leave first 64K intact)\n");
		//printf("  -i  Use internal flash (Default: use device S: for reading)\n");
		//printf("  -e  Use external flash (Default: use device S: for reading)\n");
		return S_ERROR;		
	}
	
	flash = VODEV('S');
	if (!flash || !flash->Identify) {
		printf("No S: flash\n");
		return S_ERROR;
	}
	printf("Sysrestore S: %s\n",flash->Identify(flash, NULL, 0));
	if (!strstr(flash->Identify(flash,NULL,0),"SPI")) {
		printf("Doesn't seem like SPI flash, failing.\n");
		return S_ERROR;
	}
	

	ph = (DEVICE*)(flash->hardwareInfo[0]);
	if (!flash->Identify || !ph || !ph->Identify) {
		printf("Cannot determine SPI flash/bus\n");
		return S_ERROR;
	}
	printf("Bus: %s\n",ph->Identify(ph, NULL, 0));
	ioctl(VODEV('S'), IOCTL_GET_GEOMETRY, &g);
	sectors = g.totalSectors + RESERVED_SECTORS; 
	printf("Flash size %ld sectors (%ldK)\n",sectors,sectors/2);
	flash->BlockRead(flash, 0-RESERVED_SECTORS, 1, buf);
	printf("Boot record: %04x%04x (%c%c%c%c)\n",buf[0],buf[1],buf[0]>>8,buf[0]&0xff,buf[1]>>8,buf[1]&0xff);
		
	if ((buf[0] != 0x564c) || (buf[1] != 0x5335)) {
		printf("Boot record validity check failed (VLS5 wanted), giving up.\n");
		printf("Maybe cannot read the boot record or size of reserved boot area is not 128K.\n");
		return S_ERROR;
	}
	if ((sectors < 256) || (sectors > 100000)) {
		printf("Flash size seems not correct (really small or really big)\n");
		return S_ERROR;
	}

	for (i=0; i<nParam; i++) {
		if (!strcmp(p, "-a")) {
			verbatim = 1;
			firstBlock = 0;
		} else {
			if ((strlen(p) < 3) || (p[1]!=':')) {
				printf("%s is not a valid full path file name\n",p);
				return S_ERROR;
			}
			ifp = fopen(p,"rb");		
		}
		p += strlen(p)+1;
	}
	if (!ifp) {
		printf("Cannot open input file\n");
		return S_ERROR;
	}
	
	fileSectors = (ifp->fileSize + 511) / 512;
	fileBlocks = (fileSectors + 127) / 128; //64K blocks
	printf("Size of image file is %ld sectors (%ldK)\n",fileSectors,fileSectors/2);
	if (fileSectors > sectors) {
		printf("The image file is too large for the flash.\n");
		return S_ERROR;
	}

	//firstBlock = 30;
	while ((retries < 10) && (!all_ok)) {
		Restore();
		all_ok = Verify();
		retries++;
	}
	fclose(ifp);

	
	return S_OK;
}

