/* 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 <kernel.h>
#include <clockspeed.h>
#include <scheduler.h>
#include <stdlib.h>
#include <swap.h>
#include <uartspeed.h>
#include <crc32.h>
#include "sdram.h"


#if 0
#define GENERATE_ERRORS /* For testing error output routines */
#endif

#if 0
#define UART_CONTROL
#endif

#define LOADF_VERIFY 1
#define LOADF_CRC32  2


struct Section {
  u_int32 addr;
  u_int32 size;
  u_int32 flags;
  u_int32 unused;
} section;

#define FL_READ 1
#define FL_WRITE 2
#define FL_EXECUTE 4
#define FL_BSS 8


u_int32 currentAddr = 0x80000000U;
u_int32 highestAddr = 0x0U;


int DDRLoadVRI(register const char *fileName,
	       register int flags, register int verbose) {
  int retCode = S_ERROR;
  static u_int16 fileBuf[256], ddrBuf[256];
  u_int16 c;
  u_int32 bytes;
  u_int32 t32;
  FILE *fp = NULL;
  int nSect = 0;
  u_int32 totBytes = 0;
  int swapWords = 0;
  u_int32 errors = 0, blocks = 0;
  u_int32 crc32 = 0;
  int verify = flags & LOADF_VERIFY;
  int calcCrc32 = flags & LOADF_CRC32;

  printf("%s VSRV image %s\n", verify ? "Verify" : "Load", fileName);

#ifdef GENERATE_ERRORS
  srandom(ReadTimeCount());
#endif

  if ((u_int16)fileBuf & 1) {
    printf("E: fileBuf in odd address 0x%04x\n", fileBuf);
    goto finally;
  }

  if ((u_int16)ddrBuf & 1) {
    printf("E: ddrBuf in odd address 0x%04x\n", ddrBuf);
    goto finally;
  }

  if (!(fp = fopen(fileName, "rb"))) {
    printf("File \"%s\" not found\n", fileName);
    goto finally;
  }

  bytes = fp->fileSize;
  printf("%s, %ld bytes\n", fp->Identify(fp, NULL, 0), bytes);

  c = fread(&t32, 2, 1, fp);
  crc32 = CalcCrc32(crc32, &t32, 4);	// Only valid if c == 1
  t32 = Swap32Words(t32);		// Only valid if c == 1
  if (c < 1) {
    printf("E: Empty file\n");
    goto finally;
  } else if (t32 == 0x56525649) { /* VRVI */
    swapWords = 1;
  } else if (t32 != 0x56524931) { /* VRI1 */
    printf("E: Wrong identifier %08lx, \"VRI1\" = 0x56524931\n", t32, c);
    goto finally;
  }

  while (fread(&section, sizeof(section), 1, fp) == 1) {
    u_int32 bytesLeft = section.size;
    u_int32 addr = section.addr /*& 0x0FFFFFFF*/;
    static const char fls[] = "RWEB";
    const char *p = fls;
    u_int16 bit = 1;
    int errorsInThisSection = 0, blocksInThisSection = 0;

    if (calcCrc32) {
      crc32 = CalcCrc32(crc32, &section, 2*sizeof(section));
    }

    totBytes += bytesLeft;

    if (verbose) {
      printf("Sect %2x: addr %8lx, size %7lx, flags %lx (",
	     nSect, section.addr, section.size, section.flags);
      while (*p) {
	if (bit & section.flags) printf("%c", *p);
	bit <<= 1;
	p++;
      }
      printf(")\n");
    }
    nSect++;

    memset(fileBuf, 0, 256);
    while (bytesLeft) {
      int bytesToHandle = (bytesLeft > 512) ? 512 : (s_int16)bytesLeft;
      if (bytesToHandle < 512) memset(fileBuf, 0, 256);
      //      printf(" %ld:%d", bytesLeft, bytesToHandle); Delay(1);
      if (!(section.flags & FL_BSS)) {
	fread(fileBuf, 1, (bytesToHandle+1)>>1, fp);
	if (calcCrc32) {
	  crc32 = CalcCrc32(crc32, fileBuf, bytesToHandle);
	}
      }
      blocksInThisSection++;
      blocks++;
      if (!verify) {
	if (WriteSdRam(fileBuf, addr, bytesToHandle, swapWords)) {
	  printf(" DDR write error @%08lx, %x bytes\n", addr, bytesToHandle);
	  errors++;
	  calcCrc32 = 0;
	  goto finally_errors;
	}
      } else {
	if (ReadSdRam(ddrBuf, addr, bytesToHandle, swapWords)) {
	  printf(" DDR read error @%08lx, %x bytes\n", addr, bytesToHandle);
	  errors++;
	  calcCrc32 = 0;
	  goto finally_errors;
	}
#ifdef GENERATE_ERRORS
	if (!(random() & 0xf)) {
	  ddrBuf[random() & 0xff] ^= (1<<(random()&0x1f));
	}
#endif
	if (memcmp(fileBuf, ddrBuf, bytesToHandle>>1)) {
	  errors++;
	  if (!errorsInThisSection++ && verbose) {
	    int i;
	    printf(" Verify error @%08lx (only first one for this section "
		   "reported)\n", addr);
	    for (i=0; i<8; i++) {
	      printf(" File:DDR ");
	    }
	    printf("\n");
	    for (i=0; i<256; i+=8) {
	      int j;
	      for (j=0; j<8; j++) {
		printf(" %04x%c%04x",
		       fileBuf[i+j],
		       (fileBuf[i+j] != ddrBuf[i+j]) ? '#' : ':',
		       ddrBuf[i+j]);
	      }
	      printf("\n");
	    }
	  }
	}
      }
      addr += bytesToHandle;
      bytesLeft -= bytesToHandle;
    }
    if (verify && verbose) {
      if (errorsInThisSection) {
	printf("%d/%d blocks had errors in section\n",
	       errorsInThisSection, blocksInThisSection);
      }
    }
    if ((currentAddr = addr) > highestAddr) highestAddr = addr;
  }


  printf("0x%x sections, %ld bytes in file, %ld bytes %s DDR",
	 nSect, fp->fileSize, totBytes,
	 verify ? "verified from" : "transferred to");
  if (verify) {
    printf(", %ld/%ld errors\n", errors, blocks);
  } else {
    printf("\n");
  }
  if (calcCrc32) {
    printf("File CRC32 0x%08lx, size 0x%lx\n", crc32, fp->fileSize);
  }
 finally_errors:
  if (errors) {
    printf("*** THERE WERE %ld/%ld ERRORS! ***\n", errors, blocks);
  }

  retCode = errors ? S_ERROR : S_OK;
 finally:
  if (fp) {
    fclose(fp);
    fp = NULL;
  }

  if (swapWords) {
    printf("W: File %s was in old VRVI format instead of faster-to-load "
	   "VRI1 format.\n   Update your converter!\n", fileName);
  }

  return retCode;
}





int DDRLoadBinary(register const char *fileName, register u_int32 addr,
		  register u_int16 flags, register u_int16 verbose) {
  int retCode = S_ERROR;
  static u_int16 fileBuf[256], ddrBuf[256];
  u_int16 c;
  u_int32 bytes;
  FILE *fp = NULL;
  u_int32 bytesLeft;
  u_int32 errors = 0, blocks = 0;
  u_int32 crc32 = 0;
  int verify = flags & LOADF_VERIFY;
  int calcCrc32 = flags & LOADF_CRC32;

#ifdef GENERATE_ERRORS
  srandom(ReadTimeCount());
#endif

  printf("%s binary file %s @ 0x%08lx",
	 verify ? "Verify" : "Load", fileName, addr);

  if ((u_int16)fileBuf & 1) {
    printf("\nE: fileBuf in odd address 0x%04x\n", fileBuf);
    goto finally;
  }

  if ((u_int16)ddrBuf & 1) {
    printf("\nE: ddrBuf in odd address 0x%04x\n", ddrBuf);
    goto finally;
  }

  if ((u_int16)addr & 3) {
    printf("\nE: address not aligned to 32 bits\n");
    goto finally;
  }

  if (!(fp = fopen(fileName, "rb"))) {
    printf("File \"%s\" not found\n", fileName);
    goto finally;
  }

  bytes = fp->fileSize;
  printf(", %ld bytes\n", bytes);

  bytesLeft = bytes;
  while (bytesLeft) {
    int bytesToHandle = (bytesLeft > 512) ? 512 : bytesLeft;
    int oddBytes = bytesToHandle & 3;
    bytesLeft -= bytesToHandle;
    /* bytesToHandle is now divisable by 4, but last bytes may be missing. */
    bytesToHandle &= ~3;
    fread(fileBuf, 1, bytesToHandle>>1, fp);
    blocks++;
    if (calcCrc32) crc32 = CalcCrc32(crc32, fileBuf, bytesToHandle);
    /* If there are 1-3 odd bytes, read them separately and pad to buffer. */
    if (oddBytes) {
      int i;
      for (i=0; i<4; i++) {
	u_int16 c = 0;
	if (i<oddBytes) {
	  c = fgetc(fp);
	  if (calcCrc32) crc32 = CalcCrc32Byte(crc32, c);
	}
	MemCopyPackedBigEndian(fileBuf, bytesToHandle + i, &c, 1, 1);
      }
      /* Flush registers to go around LCC 1.70 bug */
      /* Without the bugfix files of size not divisable by 4, i.e. with
	 oddBytes != 0, i.e. taking this branch, will fail. */
      __asm {
	get (bytesToHandle),a0
	  mv a0,a1
	  }
      bytesToHandle += 4;
    }
#if 0
    if (bytesToHandle < 512) {
      printf("bytesToHandle = %d, data %04x %04x %04x %04x\n",
	     bytesToHandle, fileBuf[0], fileBuf[1], fileBuf[2], fileBuf[3]);
    }
#endif
    Swap16N(fileBuf, bytesToHandle>>1);
    if (!verify) {
      if (WriteSdRam(fileBuf, addr, bytesToHandle, 0)) {
	printf(" DDR write error @%08lx, %x bytes\n", addr, bytesToHandle);
	errors++;
	calcCrc32 = 0;
	goto finally_errors;
      }
    } else {
      if (ReadSdRam(ddrBuf, addr, bytesToHandle, 0)) {
	printf(" DDR read error @%08lx, %x bytes\n", addr, bytesToHandle);
	errors++;
	calcCrc32 = 0;
	goto finally_errors;
      }
#ifdef GENERATE_ERRORS
      if (!(random() & 0xf)) {
	ddrBuf[random() & 0xff] ^= (1<<(random()&0xf));
      }
#endif

      if (memcmp(fileBuf, ddrBuf, bytesToHandle>>1)) {
	if (!errors++ && verbose) {
	  int i;
	  printf(" Verify error @%08lx (only first one for this binary "
		 "reported)\n", addr);
	  for (i=0; i<8; i++) {
	    printf(" File:DDR ");
	  }
	  printf("\n");
	  for (i=0; i<256; i+=8) {
	    int j;
	    for (j=0; j<8; j++) {
	      printf(" %04x%c%04x",
		     fileBuf[i+j],
		     (fileBuf[i+j] != ddrBuf[i+j]) ? '#' : ':',
		     ddrBuf[i+j]);
	    }
	    printf("\n");
	  }
	}
      }
    }
    addr += bytesToHandle;
    if ((currentAddr = addr) > highestAddr) highestAddr = addr;
  }

  if (verify) {
    printf(" -> %ld/%ld errors\n", errors, blocks);
  }
  if (calcCrc32) {
    printf("File CRC32 0x%08lx, size 0x%lx\n", crc32, fp->fileSize);
  }
 finally_errors:
  if (errors) {
    printf("*** THERE WERE %ld/%ld ERRORS! ***\n", errors, blocks);
  }
  retCode = errors ? S_ERROR : S_OK;
 finally:
  if (fp) {
    fclose(fp);
    fp = NULL;
  }
  return retCode;
}



void SendUartMuxByte(register s_int16 c) {
  while (PERIP(UARTMUX_STATUS) & UART_ST_TXFULL);
  PERIP(UARTMUX_DATA) = c;
}

void SendUartMuxBytes(register s_int16 *p) {
  while (*p >= 0) SendUartMuxByte(*p++);
  while (PERIP(UARTMUX_STATUS) & (UART_ST_TXFULL|UART_ST_TXRUNNING));
}

static s_int16 startupSequence[] =
  {0x10, 0x4, 0xf0, 0, 0, 0, 1, 0, 0, 0, -1};
static s_int16 stopSequence[] =
  {0x10, 0x4, 0xf0, 0, 0, 0, 0, 0, 0, 0, -1};

ioresult main(char *parameters) {
  char *p = parameters;
  int nParam;
  int verbose = 1, i;
  int retCode = S_ERROR;
  u_int32 addr = 0;
  u_int32 tc = 0;
  u_int32 errors = 0;
  u_int32 startTime = ReadTimeCount();
  int needsInit = 1;
  int verify = 0;
  int calcCrc32 = 0, sdr12MHz = 1;

#ifdef UART_CONTROL
  printf("Take over Mux Uart\n");
  PERIP(UARTMUX_DIV) = GetUartRegister(clockSpeed.cpuClkHz, 1000000);
  PERIP(VSRV1_CF) |= VSRV1_CF_MUX;
  Delay(1);
  printf("Stop Risc-V\n");
  SendUartMuxBytes(stopSequence);
#else
  printf("Disable Risc-V\n");
  PERIP(VSRV1_CF) &= ~VSRV1_CF_ENABLE;
#endif

  nParam = RunProgram("ParamSpl", parameters);

  for (i=0; i<nParam; i++) {
    if (!strcmp(p, "-h")) {
      printf("Usage: DDRLoad [-r|+r|-c|+c|-s|+s|-v|+v|-h] [-aaddr] [-bfile] "
	     "[f1.vri [...]]\n"
	     "-r|+r\tVerify VRI file after writing on|off\n"
	     "-c|+c\tCalculate file CRC32 on|off\n"
	     "-s|+s\tInitialize DDR using 12 MHz CPU on|off\n"
	     "-v|+v\tVerbose on|off\n"
	     "-aaddr\tSet address for binary load to addr (current 0x%08lx)\n"
	     "-ah\tSet address for binary load to highest seen addr (0x%08lx)\n"
	     "-bfile\tLoad binary file\n"
	     "-h\tShow this help\n"
	     "f1.vri\tLoad VRI file f1.vri\n", currentAddr, highestAddr);
      retCode = S_OK;
      goto finally;
    } else if (!strcmp(p, "-v")) {
      verbose = 1;
    } else if (!strcmp(p, "+v")) {
      verbose = 0;
    } else if (!strcmp(p, "-r")) {
      verify = 1;
    } else if (!strcmp(p, "+r")) {
      verify = 0;
    } else if (!strcmp(p, "-s")) {
      sdr12MHz = 1;
    } else if (!strcmp(p, "+s")) {
      sdr12MHz = 0;
    } else if (!strcmp(p, "-c")) {
      calcCrc32 = 1;
    } else if (!strcmp(p, "+c")) {
      calcCrc32 = 0;
    } else if (!strcmp(p, "-ah")) {
      currentAddr = highestAddr;
    } else if (!strncmp(p, "-a", 2)) {
      currentAddr = strtoul(p+2, NULL, 0);
    } else if (!strncmp(p, "-b", 2)) {
      u_int16 fl = calcCrc32 ? LOADF_CRC32 : 0;
      if (needsInit) {
	/* Take control of SD RAM */
	if (InitSdRam((verbose?SDRF_VERBOSE:0)|
		      (sdr12MHz?SDRF_USE_12_MHZ_CLOCK_FOR_INIT:0))) {
	  goto finally;
	}
	needsInit = 0;
      }
      if (DDRLoadBinary(p+2, currentAddr, fl, verbose)) goto finally;
      if (verify && DDRLoadBinary(p+2, currentAddr, fl|LOADF_VERIFY, verbose)) {
	goto finally;
      }
    } else {
      u_int16 fl = calcCrc32 ? LOADF_CRC32 : 0;
      if (needsInit) {
	/* Take control of SD RAM */
	if (InitSdRam((verbose?SDRF_VERBOSE:0)|
		      (sdr12MHz?SDRF_USE_12_MHZ_CLOCK_FOR_INIT:0))) {
	  goto finally;
	}
	needsInit = 0;
      }
      if (DDRLoadVRI(p, fl, verbose)) goto finally;
      if (verify && DDRLoadVRI(p, fl|LOADF_VERIFY, verbose)) goto finally;
    }
    p += strlen(p)+1;
  }

  if (needsInit) {
    /* Take control of SD RAM */
    if (InitSdRam((verbose?SDRF_VERBOSE:0)|
		  (sdr12MHz?SDRF_USE_12_MHZ_CLOCK_FOR_INIT:0))) {
      goto finally;
    }
    needsInit = 0;
  }

#ifdef UART_CONTROL
  printf("Hand over SD RAM to Risc-V\n");
  PERIP(SDRAM_CF) = SDRAM_CF_AXIMODE | SDRAM_CF_MMUENA;
  printf("Start Risc-V\n");
  SendUartMuxBytes(startupSequence);
  printf("Hand over Mux Uart\n");
  PERIP(VSRV1_CF) &= ~VSRV1_CF_MUX;
#else
  printf("Hand over SD RAM to Risc-V\n");
  PERIP(SDRAM_CF) = SDRAM_CF_AXIMODE | SDRAM_CF_MMUENA;
  printf("Enable Risc-V\n");
  PERIP(VSRV1_CF) |= VSRV1_CF_ENABLE;
#endif

  retCode = S_OK;
 finally:
  printf("Execution took %.3f seconds\n", 0.001*(ReadTimeCount()-startTime));
  return retCode;
}
