/* 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 VSRV 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 <uartspeed.h>
#include <vsrv.h>
#include <imem.h>
#include <vsos.h>
#include <stdlib.h>
#include <swap.h>
#include <consolestate.h>
#include "ddrsym.h"

#define MAX_DATA_SIZE 256
u_int16 dataBuf[MAX_DATA_SIZE];

struct DdrBlob ddrBlob = {
  dataBuf, 0, 0x12345678
};

void DataWrite(register u_int16 *p, register u_int16 n) {
  memcpy(dataBuf+ddrBlob.size, p, n);
  ddrBlob.size += n;
}

void hdr_write(FILE *fp, u_int16 size) {
	char pbuf[8] = "\pVLSI";
	pbuf[6] = size;

	// convert byte order of known fields.
	Swap16N((void *)&pbuf[6],1);
	DataWrite(pbuf, 8);
}

int hdr_read(FILE *fp) {
	char pbuf[8];
	fread(pbuf, 1, 8, fp);

	Swap16N((void *)&pbuf[6],1);

	if (pbuf[0] != 0x564c && pbuf[1] != 0x5349) {
		printf("Paramfile magic not found.\n");
		return -1;
	}

	if (pbuf[6] > 32) // we don't expect anything above that,
		return -2;

	printf("Got: %x\n", pbuf[6]);

	return pbuf[6];
}

struct binparam {
	u_int16 tgrp;
	u_int16 id;
	union {
		u_int32 int32;
		u_int16 ant16[4];
	} value;
};

int u32_write(FILE *fp, u_int16 id, u_int32 value) {
	struct binparam bp;

	bp.tgrp		= 0x0000;
	bp.id		= id;
	bp.value.int32 = value;
	Swap16N((void *)&bp, 4);
	DataWrite(&bp, 4);

	return 1;
}

int clk_write(FILE *fp) {
	struct binparam bp;

	/* SYSCLK */
	bp.tgrp		= 0x0000;
	bp.id		= 0x0001;
	bp.value.int32	= clockSpeed.cpuClkHz;
	Swap16N((void *)&bp, 4);
	DataWrite(&bp, 4);

	/* RVCLK */
	bp.tgrp		= 0x0000;
	bp.id		= 0x0002;
	bp.value.int32	= clockSpeed.rvClkHz;
	Swap16N((void *)&bp, 4);
	DataWrite(&bp, 4);

	return 2;
}

int prm_read(FILE *fp, struct binparam *bp) {
	printf("Pre\n"); 
	fread(bp, 1, 4, fp);
	Swap16N((void *)bp, 4);

	if (bp->tgrp == 0)			/* 8 byte param, we are done */
		return 1;

	printf("BCHK\n"); 
	if (bp->tgrp == 0x100) {		/* 12 byte param, (type:1, group:0) ie mac */
		u_int16 *p = (u_int16 *)bp+4;
		fread(p, 1, 2, fp);
		Swap16N(p, 2);

		return 1;
	}
	return -1;
}

int u48_write(FILE *fp, u_int16 id, char *value) {
	struct binparam bp;

	bp.tgrp = 0x100;		/* type:1, group:0 */
	bp.id = id;

	bp.value.ant16[0] = value[0];
	bp.value.ant16[1] = value[1];
	bp.value.ant16[2] = value[2];
	bp.value.ant16[3] = 0x8001;

	Swap16N((void *)&bp, 6);
	DataWrite(&bp, 6);

	return 1;
}

void dummy_write(FILE *fp, u_int16 t) {
	struct binparam bp;

	while (t-->0) {
		bp.tgrp		= 0x0000;
		bp.id		= 0x0000;
		bp.value.int32	= 0;
		DataWrite(&bp, 4);
	}
}

ioresult main(char *parameters) {
	char *p = parameters;
	int nParam;
	int verbose = 1, i, bp = 0;
	int retCode = S_ERROR;
	int quit = 0;
	char mac[3] = {0,0,0};
	FILE *fp = NULL;
  
	nParam = RunProgram("ParamSpl", parameters);

	for (i=0; i<nParam; i++) {
		if (!strcmp(p, "-v")) {
			verbose = 1;
		} else if (!strcmp(p, "+v")) {
			verbose = 0;
		} else if (!strncmp(p, "-p", 2)) {
			u_int16 k = atoi(p+2);
			printf("-p! %s %d\n", p, k);
		} else if (!strncmp(p, "-m", 2)) {
			// supported 00:02:b3:c8:a5:ca and 0002b3c8a5ca
			char *n = p+2;
			unsigned int tmp = 0,z=0;
			i = 0;
			while (*n != 0 && z != 3) {
				if (*n != ':') {
					tmp <<= 4;
					if (*n >'0' && *n <= '9') {
						tmp += (*n-'0');
					} else if (*n > 0x60 && *n < 0x67) {
						tmp += (*n-0x57);
					} else if (*n > 0x40 && *n < 0x47) {
						tmp += (*n-0x37);
					}
					i++;

					if (i == 4) {
						i = 0; 
						mac[z++] = tmp;
					}
				}
				n++;
			}
			if (*n != 0 || z != 3) {
				printf("Incorrect mac address format. provided: %s. Ignoring.\n", p+2);
				mac[0] = mac[1] = mac[2] = 0;
			} else if (verbose) {
				printf("MAC ADDR set to %04x%04x%04x\n", mac[0], mac[1], mac[2]);
			}

		} else if (!strcmp(p, "-h")) {
			printf("Usage: RVParam\n"
				"-h\tShow this unhelpful help\n"
				);
			goto finally_ok;
		} else {
			printf("E: Unknown parameter \"%s\".\n", p);
			goto finally;
		}
		p += strlen(p)+1;
	}

	
	if (!nParam) {
	  struct binparam bp;
	  int k;
	  int ret = 0;
	  
	  fp = fopen("rvparam.bin", "rb");

	  if (!fp) {
	    printf("File open failed\n");
	    goto finally;
	  }

	  k = hdr_read(fp);
	  printf("k=%d\n", k);
	
	  while (k-- > 0 && ret >= 0) {
	    printf("Reading %d\n", k);
	    ret = prm_read(fp, &bp);
	    printf("Prm: %x %x\n", bp.tgrp, bp.id);
	  }
	  printf("Done\n");

	  fclose(fp);
	  fp = NULL;

	}

	/* Do the write always. */
	{
	  
	  bp = 2 /* clocks */ + 1 /* memory size */ + 1 /* mac addr */;

	  hdr_write(fp, bp);
	  bp -= clk_write(fp);				/* two clocks  */
	  bp -= u32_write(fp, 0x8000, 0x11223344);		/* Memory size */
	  ddrBlob.memSize = dataBuf+ddrBlob.size-2; /* Set memory pointer. */
	  bp -= u48_write(fp, 0x42AF, &mac);		/* mac addr */

	  dummy_write(fp, bp);

#if 0
	  for (i=0; i<ddrBlob.size; i++) {
	    printf("data[%2d] = %04x\n", i, dataBuf[i]);
	  }
#endif
	}

finally_ok:
	retCode = S_OK;
finally:
	return retCode;
}

ioresult init(char *parameters) {
  struct ExtSymbol __mem_y *exchangeSym = SymbolAdd(DDR_SYM_NAME, main, (u_int16)(&ddrBlob));
#if 0
  printf("RVParam: ddrBlob is at 0x%x\n", &ddrBlob);
  printf("RVParam: Made symbol " DDR_SYM_NAME " at 0x%x\n", exchangeSym);
  Delay(100);
#endif
  return S_OK;
}

void fini(void) {
#if 0
  printf("Exiting RVParam, *memSize = 0x%08lx\n", *ddrBlob.memSize);
#endif
  SymbolDeleteLib(main);
}
