/* 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 <string.h>
#include <strings.h>
#include <kernel.h>
#include <clockspeed.h>
#include <sysmemory.h>

const char IXY[3] = "IXY";

#define BADMAP_ADDR (0x20000-0x1000)

struct Bad {
  u_int16 id[2];
  u_int16 badMap[3];
  u_int16 errors[3];
  u_int16 d[2048-3-3-2];
} bad;


/*
  Output format structure:
  All fields are in 16-bit Big-Endian format.

  2	ID	0x0bad 0xb175
  3	BADMAPS	I,X,Y 4k bad maps, each bit indicates a 4K bad page
  3	ERRORS  I,X,Y number of errors
  EIXY	EAIXY	I,X,Y error addresses
  1	CHECK	Trivial checksum (0xaaaa ^ word[0] ^ ... ^ word[n-1])

  Example file:

  0000  0b ad b1 75 00 e0 00 00  00 00 00 05 00 00 00 00  |...u............|
  0010  dd bc 4d fc 4d 7c 4c 5c  47 b8 c6 4f              |..M.M|L\G..O|

   0- 3	ID	0x0bad 0xb175
   4- 5	BADMAPI	0x00e0 = bits 5,6,7 set, so pages 5,6,7 marked bad
  		Page 5 = 0x5000-0x5fff
  		Page 6 = 0x6000-0x6fff
  		Page 7 = 0x7000-0x7fff
   6- 7	BADMAPX 0x0000 = No bad pages in X memory
   8- 9	BADMAPY 0x0000 = No bad pages in Y memory
   a- b	ERRORSI 0x0005 = 5 errors in I memory
   c- d	ERRORSX 0x0000 = 0 errors in X memory
   e- f	ERRORSY 0x0000 = 0 errors in Y memory
  10-19	EAI	0xddbc 0x4dfc 0x4d7c 0x4c5c 0x47b8 = I error addresses
	EAX/EAY	If there were X/Y error addresses, they'd be here
  1a-1b CHECK	0xc64f = Trivial checksum for rudimanetary data verification

  File size is 18+2*(ERRORS) bytes, in this case 18+2*5 = 28 = 0x1c bytes.

 */








#define MIN(a,b) (((a)<(b))?(a):(b))

#define DEFAULT_FLASH_MIB 2

u_int16 flashMiB = DEFAULT_FLASH_MIB;


#define SPI_MASTER_8BIT_CSLO()   (PERIP(SPI0_CF) =	\
		SPI_CF_MASTER | SPI_CF_DLEN8 | SPI_CF_FSIDLE0)
#define SPI_MASTER_16BIT_CSLO()  (PERIP(SPI0_CF) =	\
		SPI_CF_MASTER | SPI_CF_DLEN16 | SPI_CF_FSIDLE0)
#define SPI_MASTER_8BIT_CSHI()   (PERIP(SPI0_CF) =	\
		SPI_CF_MASTER | SPI_CF_DLEN8 | SPI_CF_FSIDLE1)
#define SPI_MASTER_16BIT_CSHI()  (PERIP(SPI0_CF) =	\
		SPI_CF_MASTER | SPI_CF_DLEN16 | SPI_CF_FSIDLE1)


#define WORDS_PER_IFLASH_PAGE 128
#define IFLASH_PAGES (1L<<(20-8))

#define IFLASH_ST_WRITE_IN_PROGRESS_B	0
#define IFLASH_ST_WRITE_ENA_B		1
#define IFLASH_ST_BP0_B			2
#define IFLASH_ST_BP1_B			3
#define IFLASH_ST_BP2_B			4
#define IFLASH_ST_STATUS_WRITE_DIS_B	7

#define IFLASH_ST_WRITE_IN_PROGRESS	(1<<IFLASH_ST_WRITE_IN_PROGRESS_B)
#define IFLASH_ST_WRITE_ENABLE		(1<<IFLASH_ST_WRITE_ENABLE_B)
#define IFLASH_ST_BP0			(1<<IFLASH_ST_BP0_B)
#define IFLASH_ST_BP1			(1<<IFLASH_ST_BP1_B)
#define IFLASH_ST_BP2			(1<<IFLASH_ST_BP2_B)
#define IFLASH_ST_STATUS_WRITE_DISABLE	(1<<IFLASH_ST_STATUS_WRITE_DISABLE_B)

#define SPI_EEPROM_COMMAND_WRITE_ENABLE  0x06
#define SPI_EEPROM_COMMAND_WRITE_DISABLE  0x04
#define SPI_EEPROM_COMMAND_READ_STATUS_REGISTER  0x05
#define SPI_EEPROM_COMMAND_WRITE_STATUS_REGISTER  0x01
#define SPI_EEPROM_COMMAND_READ  0x03
#define SPI_EEPROM_COMMAND_WRITE 0x02
#define SPI_EEPROM_COMMAND_AAI_WORD_PROGRAM 0xAD /* For SST25VF016B and like */
//#define SPI_EEPROM_COMMAND_CLEAR_ERROR_FLAGS 0x30
//#define SPI_EEPROM_COMMAND_PARAMETER_BLOCK_ERASE 0x40
#define SPI_EEPROM_COMMAND_ERASE_SECTOR 0x20 /*  4 KiB */
#define SPI_EEPROM_COMMAND_ERASE_BLOCK  0xD8 /* 64 KiB */
#define SPI_EEPROM_COMMAND_ERASE_CHIP 0xC7   /*    all */









auto u_int16 ReadIFlashStatus(void) {
  u_int16 st;

#if 0
  /* Give the device a bit of time to settle. */
  SpiSendReceive(0);
  SpiSendReceive(0);
#endif

  /* Now do the actual operation. */
  SPI_MASTER_8BIT_CSLO();
  SpiSendReceive(SPI_EEPROM_COMMAND_READ_STATUS_REGISTER); // RDSR, page 38
  st = SpiSendReceive(0);
  SPI_MASTER_8BIT_CSHI();
  return st;
}


auto u_int16 ReadIFlashPage(register u_int16 *d, u_int16 pageAddr) {
  u_int16 i;

  while (ReadIFlashStatus() & IFLASH_ST_WRITE_IN_PROGRESS)
    ;
  DelayMicroSec(1); /* CS must stay high long enough for device to notice. */

  SPI_MASTER_8BIT_CSLO();
  SpiSendReceive(0x0b);	// FAST_READ cmd=0x0b, page 93
  SPI_MASTER_16BIT_CSLO();
  SpiSendReceive(pageAddr);	// Bits 23:8 of address
  SpiSendReceive(0);		// Bits  7:0 of address + dummy byte

  for (i=0; i<WORDS_PER_IFLASH_PAGE; i++) {
    *d++ = SpiSendReceive(0);
  }
  SPI_MASTER_8BIT_CSHI();
  return 0;
}








const u_int16 __y spiSpeeds[10] = {
  80, 40, 20, 12, 8, 6, 4, 3, 2, 1
};
u_int16 spiSpeedIdx = 6;




s_int16 InitFlash(register s_int16 verbose) {
  int i;
  int retCode = -1;

  /* Make SPI0 pins SPI peripheral */
  PERIP(GPIO0_PERIPMODE) |=
    (GPIO0_SPI0_XCS|GPIO0_SPI0_SCLK|GPIO0_SPI0_MISO|GPIO0_SPI0_MOSI);
  PERIP(GPIO0_DDR) &=
    ~(GPIO0_SPI0_XCS|GPIO0_SPI0_SCLK|GPIO0_SPI0_MISO|GPIO0_SPI0_MOSI);
  
  /* SPI init */
  PERIP(SPI0_CF) = SPI_CF_MASTER | SPI_CF_DLEN8 | SPI_CF_FSIDLE1;
  PERIP(SPI0_FSYNC) = 0;
  PERIP(SPI0_CLKCF) = SPI_CC_CLKDIV * (spiSpeeds[spiSpeedIdx]-1);
  if (verbose) printf("SPI speed %2.1f Mbit/s\n",
		      clockSpeed.cpuClkHz / spiSpeeds[spiSpeedIdx] *
		      (1.0/1000000.0/2.0));
  
  /* Run some empty clock cycles without chip select. */
  SPI_MASTER_16BIT_CSHI();
  for (i=0; i<10; i++) {
    SpiSendReceive(0xFF);
  }

#if 0
  {
    u_int16 rems; /*REMS*/
    /* Read REMS: Datasheet page 25: Send 0x90, two dummy bytes, address byte*/
    SPI_MASTER_8BIT_CSLO();
    SpiSendReceive(0x90);
    SPI_MASTER_16BIT_CSLO();
    SpiSendReceive(0);
    SPI_MASTER_8BIT_CSLO();
    SpiSendReceive(0);
    SPI_MASTER_16BIT_CSLO();
    rems = SpiSendReceive(0);
    SPI_MASTER_8BIT_CSHI();
  
    if (verbose) printf("  REMS: %4x\n", rems);

    if (rems == 0xFFFFU || !rems) {
      if (verbose) printf("    Warning: Can't find REMS\n");
      /*      goto noFlash;*/
    }
  }

  {
    u_int16 rdid[3]; /*RDID*/
    /* Read RDID: Datasheet page 25: Send 0x9F, read 3 bytes */
    SPI_MASTER_8BIT_CSLO();
    SpiSendReceive(0x9F);
    rdid[0] = SpiSendReceive(0);
    rdid[1] = SpiSendReceive(0);
    rdid[2] = SpiSendReceive(0);
    SPI_MASTER_8BIT_CSHI();
  
    if (verbose) printf("  RDID: manuf. %2x, type %2x, "
			"dens. %2x\n", rdid[0], rdid[1], rdid[2]);
    
    if ((rdid[0]&rdid[1]&rdid[2]) == 0xFF || !(rdid[0]|rdid[1]|rdid[2])) {
      if (verbose) printf("    Warning: Can't find RDID\n");
      /*      goto noFlash; */
    } else {
      if (verbose) {
	printf("    %ld MiB = %ld KiB = %ld B\n",
	       1L<<(rdid[2]-20), 1L<<(rdid[2]-10), 1L<<rdid[2]);
      }
      flashMiB = 1<<(rdid[2]-20);
    }
  }
#endif
  
  retCode = 0;
 finally:
  return retCode;
}



void (*allocMemFuncs[3])(u_int16 addr, size_t size) = {
  AllocMemAbsI, AllocMemAbsX, AllocMemAbsY
};


ioresult main(char *parameters) {
  int nParam, i, ixy;
  char *p = parameters;
  int retVal = S_ERROR;
  int verbose = 0;
  FILE *fp;
  u_int16 *d = NULL;
  u_int16 checksum = 0xaaaa;
  u_int16 errors = 0;
  u_int16 fatal = 0;

  nParam = RunProgram("ParamSpl", parameters);

  for (i=0; i<nParam; i++) {
    if (!strcmp(p, "-h")) {
      printf("Usage: BadBit [-h|-v|+v|+f|-f]\n"
	     "-f|+f\tFatal errors on|off\n"
	     "-v|+v\tVerbose on|off\n"
	     "-h\tShow this help\n");
      goto finally_ok;
    } else if (!strcmp(p, "-f")) {
      fatal = 1;
    } else if (!strcmp(p, "+f")) {
      fatal = 0;
    } else if (!strcmp(p, "-v")) {
      verbose = 1;
    } else if (!strcmp(p, "+v")) {
      verbose = 0;
    } else {
      printf("E: Unknown parameter \"%s\"\n", p);
      goto finally;
    }
    p += strlen(p)+1;
  }

  if (InitFlash(verbose)) {
    printf("F: No FLASH found\n");
    errors++;
    goto finally;
  }

  for (i=0; i<4096/256; i++) {
    ReadIFlashPage(((u_int16 *)(&bad))+i*WORDS_PER_IFLASH_PAGE, (BADMAP_ADDR/256)+i);
  }
  if (verbose) {
    printf("ID  %04x %04x\n", bad.id[0], bad.id[1]);
    printf("%s I:%04x X:%04x Y:%04x\n",
	   "MAP", bad.badMap[0], bad.badMap[1], bad.badMap[2]);
    printf("%s I:%04x X:%04x Y:%04x\n", "ERR",
	   bad.errors[0], bad.errors[1], bad.errors[2]);
  }
  if (bad.id[0] != 0xbad || bad.id[1] != 0xb175) {
    printf("ID error, ID = %04x %04x\n", bad.id[0], bad.id[1]);
    errors++;
    goto finally;
  }
  d = (u_int16 *)&bad;
  while (d < bad.d) {
    checksum ^= *d++;
  }
  for (ixy=0; ixy<3; ixy++) {
    for (i=0; i<bad.errors[ixy]; i++) {
      u_int16 addr = *d++;
      checksum ^= addr;
#if 0
      if (verbose) printf("  %c:%04x\n", IXY[ixy], addr);
#endif
    }
  }
#if 0
  if (verbose) printf("Check %04x %04x\n", checksum, *d);
#endif
  if (checksum != *d) {
    printf("E: Checksum mismatch: calculated %04x, should be %04x\n",
	   checksum, *d);
    errors++;
    goto finally;
  }

  for (ixy=0; ixy<3; ixy++) {
    for (i=15; i>=0; i--) {
      if (bad.badMap[ixy] & (1<<i)) {
	u_int16 addr = i*0x1000;
	void *(*currentAllocMemFunc)(u_int16 add, size_t siz) =
	  allocMemFuncs[ixy];
	u_int16 rAddr = currentAllocMemFunc(addr, 0x1000);
	static char ss[65];
	char *ssp = ss+64;
	if (verbose) printf("  %c:%04x-%04x: ",
			    IXY[ixy], addr, addr+0xfff);
	sprintf(ss, "+");
	if (!rAddr) {
	  /* No big chunk available? Try at least to catch something in
	     smaller pieces. */
	  int k;
	  errors++;
	  *ssp-- = '\0';
	  for (k=63; k>=0; k--) {
	    *ssp-- = currentAllocMemFunc(addr+64*k, 0x40) ? '+' : '-';
	  }
	}
      if (verbose) printf("%s\n", ss);
      }
    }
  }
  d = bad.d;
  for (ixy=0; ixy<3; ixy++) {
    for (i=0; i<bad.errors[ixy]; i++) {
      u_int16 addr = *d++;
      void *(*currentAllocMemFunc)(u_int16 add, size_t siz) =
	allocMemFuncs[ixy];
      u_int16 rAddr = currentAllocMemFunc(addr, 4);
      if (verbose) printf("  %c:%04x-%04x: %c\n",
			  IXY[ixy], addr, addr+3, rAddr ? '+' : '-');
      if (!rAddr) errors++;
    }
  }

  

  if (errors) {
    if (fatal) {
      Delay(100);
      SysFatalError("%d FATAL ERRORS.", errors);
      while (1) Delay(10);
    }
    if (verbose) printf("W: %d allocation fails\n", errors);
  }
  
 finally_ok:
  retVal = S_OK;
 finally:
  return retVal;
}
