/****************************************************************************/
/* Copyright 2003 MBARI														*/
/****************************************************************************/
/* Summary	: Diagnostic Routines for OASIS3 Mooring Controller				*/
/* Filename : diag.c														*/
/* Author	: Robert Herlien (rah)											*/
/* Project	: OASIS Mooring Replacement (OASIS3)							*/
/* Revision : 1.0															*/
/* Created	: 10/14/2002													*/
/*																			*/
/* MBARI provides this documentation and code "as is", with no warranty,	*/
/* express or implied, of its quality or consistency. It is provided without*/
/* support and without obligation on the part of the Monterey Bay Aquarium	*/
/* Research Institute to assist in its use, correction, modification, or	*/
/* enhancement. This information should not be published or distributed to	*/
/* third parties without specific written permission from MBARI.			*/
/*																			*/
/****************************************************************************/
/* Modification History:													*/
/* 03sept2003 rah - created from former O3 test routines					*/
/****************************************************************************/

#include <mbariTypes.h>					/* MBARI type definitions			*/
#include <mbariConst.h>					/* MBARI constants					*/
#include <oasis.h>						/* OASIS controller definitions		*/
#include <io.h>							/* OASIS I/O definitions			*/
#include <spi.h>						/* OASIS SPI I/O definitions		*/
#include <serial.h>						/* OASIS Serial I/O definitions		*/
#include <list.h>						/* OASIS Linked List library		*/
#include <sem.h>						/* OASIS Semaphore library			*/
#include <task.h>						/* OASIS task dispatcher			*/
#include <drvr.h>						/* OASIS driver functions			*/
#include <log.h>						/* OASIS data logging definitions	*/
#include <utils.h>						/* OASIS Utility routines			*/
#include <usrcmds.h>					/* User Command function definitions*/
#include <diag.h>						/* OASIS diagnostic routines		*/
#include <sensors.h>					/* OASIS sensor module				*/

#include <cfxbios.h>					/* Persistor BIOS definitions		*/
#include <cfxpico.h>					/* Persistor PicoDOS definitions	*/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

#define NAMESIZE		32
#define BUFSIZE			256
#define DRIVE			DSD_FIRST_DEV

Void ping_drv(Void);
Void test_drv(Driver *dp);
Void heartbeat_drv(Void);
Void  formatDrive(char *buffer);


/********************************/
/*		External Data			*/
/********************************/

Extern LstHead			drv_list;
Extern MBool			diagnosticMode;
Extern Parm_t			con_dsc;		/* Disconnect character for CONNECT */
Extern Nat16			serPort;


/********************************/
/*		Module Local Data		*/
/********************************/

//typedef enum {NOT_INIT, INACTIVE, ACTIVE} drvrState;
MLocal MBool	pingDrvActive = FALSE;
MLocal MBool	heartbeatDrvActive = FALSE;
MLocal Driver	*pingDrv = NULL;
MLocal Driver	*hbDrv = NULL;
MLocal Driver	*dummyDrv = NULL;
MLocal char		nameBuf[NAMESIZE];

MLocal DrvDesc pingDriver = 
{ "pingDriver", ping_drv, null_func,
  30, NO_SERIAL, 0, 0, 0, LOG_COMMENT, 0, 5, 0, 0, 0 };

MLocal DrvDesc heartbeatDriver = 
{ "heartbeatDriver", heartbeat_drv, null_func,
  NAT32_MAX, NO_SERIAL, 0, 0, 0, LOG_COMMENT, 0, 5, 0, 0, 0 };

MLocal DrvDesc testDriver = 
{ "TestDriver", test_drv, null_func,
  NAT32_MAX, NO_SERIAL, 9600, (NO_PTY | BIT8 | STOP1 | AUTOCR), 0, 
  LOG_COMMENT, 0, 5, 0, 0, 0 };

MLocal DrvDesc dummyDriver = 
{ "Dummy", null_func, null_func,
  NAT32_MAX, NO_SERIAL, 9600, (NO_PTY | BIT8 | STOP1 | AUTOCR), 0, 
  LOG_COMMENT, 0, 5, 0, 0, 0 };

MLocal union
{
  Byte			raw[512];
  Fat16BR		bootRec;
  Fat32BR		bootRec32;
} formatBuf;


/************************************************************************/
/* Function	   : getDecInput											*/
/* Purpose	   : Get user input, interpret as decimal value				*/
/* Inputs	   : Buffer pointer											*/
/* Outputs	   : Decimal value or ERROR									*/
/************************************************************************/
		MLocal Int16
getDecInput(char *buffer)
{
  Int16 val;

  if ((xgets_tmout(buffer, BUFSIZE, 30) == ERROR) ||
	  (sscanf(buffer, "%d", &val) < 1))
  {
	xputs("Bad input value\n");
	return(ERROR);
  }

  return(val);

} /* getDecInput() */


/************************************************************************/
/* Function	   : getHexInput											*/
/* Purpose	   : Get user input, interpret as hex value					*/
/* Inputs	   : Buffer pointer											*/
/* Outputs	   : OK or ERROR											*/
/************************************************************************/
		MLocal Errno
getHexInput(char *buffer, Nat32 *valp)
{
  if ((xgets_tmout(buffer, BUFSIZE, 30) == ERROR) ||
	  (sscanf(buffer, "%lx", valp) < 1))
  {
	xputs("Bad input value\n");
	return(ERROR);
  }

  return(OK);

} /* getHexInput() */


/************************************************************************/
/* Function	   : getPortNumber											*/
/* Purpose	   : Get user input for a serial or power port number (1-31)*/
/* Inputs	   : Buffer pointer											*/
/* Outputs	   : Decimal value or ERROR									*/
/************************************************************************/
		MLocal Int16
getPortNumber(char *buffer)
{
  Int16 val;

  xputs("Port number (1-31):  ");

  if ((val = getDecInput(buffer)) == ERROR)
	return(ERROR);

  if ((val <= 0) || (val >= 32))
  {
	xputs("Port number must be between 1 and 31\n");
	return(ERROR);
  }

  return(val);

} /* getPortNumber() */


/************************************************************************/
/* Function	   : createDriver											*/
/* Purpose	   : Create a Driver										*/
/* Inputs	   : Ptr to DrvDesc											*/
/* Outputs	   : Driver Ptr												*/
/************************************************************************/
		MLocal Driver *
createDriver(const DrvDesc *ddp)
{
  Driver		*dp;

  if ((dp = drvr_create(ddp)) == NULL)
	xputs("Driver create failed\n");
  else if (task_create(dp->drv_task, dp, dp->drv_name) == NULL)
	xputs("Task create failed\n");
  else
	xprintf("Creating driver %s at 0x%x, Ser Port %d\n",
			dp->drv_name, dp, dp->drv_parms[SER_PORT]);

  return(dp);

} /* createDriver() */


/************************************************************************/
/* Function	   : printAtoD												*/
/* Purpose	   : Print A/D voltage										*/
/* Inputs	   : A/D channel, value to print							*/
/* Outputs	   : Driver Ptr												*/
/************************************************************************/
		MLocal Void
printAtoD(Nat16 chan, Nat16 val)
{
  xprintf("A/D channel %d = %d (0x%x), %5.3f volts\n", chan, val, val,
		  (float)val * AD_SCALE);

} /* printAtoD() */


/************************************************************************/
/* Function	   : o3Diagnostic											*/
/* Purpose	   : Hardware diagnostic routines for OASIS3				*/
/* Inputs	   : pmask = 0xffff if invoked from OASIS.CFG (i.e., before */
/*				 drivers are spawned).	pmask = 0-7 otherwise			*/
/* Outputs	   : ERROR, or never returns								*/
/* Comments	   : If it passes initial checks, this function won't return*/
/*				 except by doing a BIOSReset()							*/
/************************************************************************/
		Int16
o3Diagnostic(Nat16 pmask)
{
  TaskDesc		*ourTd, *td;
  Reg Driver	*dp, *dp1;
  Nat32			lngval;
  Int16			c, i;
  Nat16			val;
  char			inbuf[BUFSIZE];

  if (((ourTd = task_get()) == NULL) || (serPort != CPU_SER))
  {
	xputs("You can only invoke the diagnostics from the main user console\n");
	return(ERROR);
  }

  if (pmask != 0xffff)
  {
	xputs("Running the O3 diagnostics will kill all driver tasks,\n");
	xputs("	 stop all data logging, and can erase all log data\n\n");
	xputs("Are you VERY, VERY sure you want to do this?\n");
	xputs("If you really want to do it, type \"XYZZY\"\n"  );
	xgets_tmout(inbuf, sizeof(inbuf), 30);
	if (cmp_ulc(inbuf, "xyzzy") == NULL)
	  return(ERROR);
  }

  /* Kill all driver tasks except for ourself (presumeably UserIF)		*/
  for (dp = (Driver *)(drv_list.lst_head); dp != DRV_NULL; dp = dp1)
  {
	dp1 = dp->drv_next;
	if (dp->drv_td != ourTd)
	  drvdel(1, dp->drv_name);
  }

  /* Kill the root (oasis supervisor) task to keep from scheduling anything*/
  if (pmask != 0xffff)
	  if ((td = taskGetInitTask()) != NULL)
		task_kill(td);

  logFlush();							/* Flush log file				*/
  io_power(0);							/* Turn off all power			*/
  diagnosticMode = TRUE;
  for (i = 1; i < PHYS_SER_PORTS; i++)	/* Open all serial relays		*/
	io_serRelay(i, FALSE);

  while(1)
  {
	xputs("\n\nType 'P' to set the state of the power relays\n");
	xputs("Type 'J' to select (close) serial relay\n");
	xputs("Type 'K' to open serial relay\n");
	xputs("Type 'L' to select (close) power relay\n");
	xputs("Type 'M' to open power relay\n");

	xputs("Type 'C' to cycle a serial relay on/off\n");
	xputs("Type 'Q' to cycle a power relay on/off\n");
	xputs("Type 'S' to pulse an SPI bit on the CPU board\n");

	xputs("Type 'I' to interactively talk to a serial port\n");
	xputs("Type 'T' to spawn task to send test msgs to a serial port\n");
	xputs("Type 'Z' to spawn/kill task to ping watchdog every 30 seconds\n");
	xputs("Type 'H' to spawn/kill task to ping heartbeat\n");
	xputs("Type 'V' to read wake bit\n");

	xputs("Type '0-7' to read corresponding A/D channel\n");
	xputs("Type '8' to read all A/D channels\n");
	xputs("Type 'A' to continuously read all A/D channels\n");

	xputs("Type 'F' to reformat the Compact Flash Disk\n");
	xputs("Type 'X' to exit and reset the system\n");
	xputs("Type 'Y' to exit to PicoDOS\n");

	while ((c = xgetc()) == ERROR)
	  task_delay(1);

	switch(c)
	{
	  case 'x':
	  case 'X':
		xputs("Goodbye\n");
		BIOSReset();
		break;

	  case 'y':
	  case 'Y':
		BIOSResetToPicoDOS();
		break;

	  case 'p':
	  case 'P':
		xputs("New Power Vector:  ");
		if (getHexInput(inbuf, &lngval) == OK)
		{
		  xprintf("\nSetting power vector to 0x%x\n", lngval);
		  io_power(lngval);
		}
		break;

	  case 'J':
	  case 'j':
		if ((i = getPortNumber(inbuf)) != ERROR)
		  io_serRelay(i, TRUE);
		break;

	  case 'K':
	  case 'k':
		if ((i = getPortNumber(inbuf)) != ERROR)
		  io_serRelay(i, FALSE);
		break;

	  case 'L':
	  case 'l':
		if ((i = getPortNumber(inbuf)) != ERROR)
		  io_pwron(i);
		break;

	  case 'M':
	  case 'm':
		if ((i = getPortNumber(inbuf)) != ERROR)
		  io_pwroff(i);
		break;

	  case 'C':
	  case 'c':
		if ((i = getPortNumber(inbuf)) != ERROR)
		{
		  xputs("Hit any key to stop\n");
		  while (xgetc_tmout(0) == ERROR)
		  {
			io_serRelay(i, TRUE);
			task_delay(TICKS_PER_SECOND/4);
			io_serRelay(i, FALSE);
			task_delay(TICKS_PER_SECOND/4);
		  }
		}
		break;

	  case 'Q':
	  case 'q':
		if ((i = getPortNumber(inbuf)) != ERROR)
		{
		  xputs("Hit any key to stop\n");
		  while (xgetc_tmout(0) == ERROR)
		  {
			io_pwron(i);
			task_delay(TICKS_PER_SECOND/4);
			io_pwroff(i);
			task_delay(TICKS_PER_SECOND/4);
		  }
		}
		break;

	  case 'S':
	  case 's':
		xputs("SPI bit to pulse (4-31 for first PIO, 36-63 for 2nd PIO):  ");
		if ((i = getDecInput(inbuf)) != ERROR)
		  if ((i >= 4) && (i <= 31))
		  {
			spiSetPIOBit(0, i, 1);
			task_delay(RELAY_DELAY);
			spiSetPIOBit(0, i, 0);
		  }
		  else if ((i >= 36) && (i <= 63))
		  {
			i -= 32;
			spiSetPIOBit(1, i, 1);
			task_delay(RELAY_DELAY);
			spiSetPIOBit(1, i, 0);
		  }
		break;


	  case 'I':
	  case 'i':
		if ((i = getPortNumber(inbuf)) != ERROR)
		{
		  if (dummyDrv == NULL)
			if ((dummyDrv = drvr_create(&dummyDriver)) == NULL)
			  break;
		  dummyDrv->drv_parms[SER_PORT] = i;
		  xputs("Type CTRL-D to end\n");
		  con_dsc = CTRL_D;
		  connect(1, "Dummy", 0, 0, 0, inbuf);
		}
		break;

	  case 'T':
	  case 't':
		if ((i = getPortNumber(inbuf)) != ERROR)
		{
		  testDriver.dd_parms[SER_PORT] = i;
		  sprintf(nameBuf, "TestDriver%d", i);
		  testDriver.dd_name = nameBuf;
		  createDriver(&testDriver);
		  xprintf("\n%s running on serial port %d\n", nameBuf, i);
		  xprintf("Type CTRL_D on serial port %d to stop\n", i);
		}
		break;

	  case 'Z':
	  case 'z':
		if (pingDrv == NULL)
		  pingDrv = createDriver(&pingDriver);

		pingDrvActive = !pingDrvActive;
		break;


	  case 'H':
	  case 'h':
		if (hbDrv == NULL)
		  hbDrv = createDriver(&heartbeatDriver);

		heartbeatDrvActive = !heartbeatDrvActive;
		break;

	  case 'V':
	  case 'v':
		xprintf("\nWake input = %1d\n", PinRead(IRQ5));
		break;

	  case 'F':
	  case 'f':
		formatDrive(inbuf);
		break;

	  case 'a':
	  case 'A':
		newline();
		for (i = 0; (xgetc_tmout(0) == ERROR); i++)
		{
		  i %= 8;
		  get_analog(i, 1, 100, &val);
		  printAtoD(i, val);
		}
		break;

	  case '8':
		newline();
		for (i = 0; i < 8; i++)
		{
		  get_analog(i, 1, 100, &val);
		  printAtoD(i, val);
		}
		break;

	  default:
		if ((c >= '0') && (c <= '7'))
		{
		  newline();
		  i = c - '0';
		  get_analog(i, 1, 100, &val);
		  printAtoD(i, val);
		}
		dispatch();
		break;
	}
  }

  return(0);

} /* o3Diagnostic() */


/************************************************************************/
/* Function	   : ping_drv												*/
/* Purpose	   : Watchdog Pinger										*/
/* Inputs	   : Driver pointer											*/
/* Outputs	   : None													*/
/************************************************************************/
		MLocal Void
ping_drv(Void)
{
  while (pingDrvActive)
  {
	ioPingWatchdog();
	task_delay(30 * TICKS_PER_SECOND);
  }

} /* ping_drv() */


/************************************************************************/
/* Function	   : heartbeat_drv											*/
/* Purpose	   : HeartBeat Pinger										*/
/* Inputs	   : Driver pointer											*/
/* Outputs	   : None													*/
/************************************************************************/
		MLocal Void
heartbeat_drv(Void)
{
  MBool			hb = TRUE;

  while (heartbeatDrvActive)
  {
	ioSetHeartbeat(hb);
	hb = !hb;
	task_delay(TICKS_PER_SECOND/2);
  }

  ioSetHeartbeat(FALSE);
  hbDrv = NULL;

} /* heartbeat_drv() */


/************************************************************************/
/* Function	   : test_drv												*/
/* Purpose	   : Test Driver											*/
/* Inputs	   : None													*/
/* Outputs	   : None													*/
/************************************************************************/
		MLocal Void
test_drv( Driver *dp )
{
  long	loopcnt = 0L;

  drv_ser_port(dp);
  xprintf("\n%s Startup, driver ptr = %lx\n", dp->drv_name, dp);

  while (xgetc_tmout(0) != CTRL_D)
  {
	xprintf("%s	 Loop count %lu\n", dp->drv_name, loopcnt++);
	task_delay(50);
  }

  drv_ser_release(dp);

} /* test_drv() */


/************************************************************************/
/* Function	   : formatDrive											*/
/* Purpose	   : Format the CFM Card in drive C							*/
/* Inputs	   : Buffer to use											*/
/* Outputs	   : None													*/
/************************************************************************/
Void  formatDrive(char *buffer)
{
  Nat32			sectors, sectsPerFat, dataClusters;
  Byte			sectorsPerCluster;
  Nat16			bytesPerSect, rsvdSects, dirEntries;
  Nat16			FAT1Sector, FAT2Sector, dirSector, dirSects;
  Nat16			FATEntriesPerSect, dataSector, i;
  Int16			val;

  xputs("\nThis will reformat a CFM card that already has an existing format.\n");
  xputs("It re-uses many existing format parameters,\n");
  xputs("and merely resets cluster size and root directory entries.\n\n");
  xputs("If you want to format an unformatted CFM card,\n");
  xputs("type '0' to the next question, type 'Y' to exit to PicoDOS,\n");
  xputs("then use the PicoDOS \"format\" program.\n");
  xputs("You can then return here to reformat it.\n\n");

  DSDReadSectors(DRIVE, 0L, &formatBuf, 1);
  sectors = (formatBuf.bootRec.fatBR.sectorsSmall_hi << 8) + 
			 formatBuf.bootRec.fatBR.sectorsSmall_low;
  if (sectors == 0)
	sectors = swapLong(formatBuf.bootRec.fatBR.numberSectors);

  rsvdSects = swapWord(formatBuf.bootRec.fatBR.reservedSectors);
  bytesPerSect = (formatBuf.bootRec.fatBR.bytesPerSect_hi << 8) + 
				  formatBuf.bootRec.fatBR.bytesPerSect_low;
  sectorsPerCluster = formatBuf.bootRec.fatBR.sectorsPerCluster;
  sectsPerFat = swapWord(formatBuf.bootRec.fatBR.sectorsPerFat);
  if (sectsPerFat == 0)
	sectsPerFat = swapLong(formatBuf.bootRec32.sectsPerFat32);

  dirEntries = (formatBuf.bootRec.fatBR.rootDirEntries_hi << 8) + 
				formatBuf.bootRec.fatBR.rootDirEntries_low;
  FAT1Sector = rsvdSects;
  FAT2Sector = FAT1Sector + sectsPerFat;
//	dirSector = FAT2Sector + sectsPerFat;
  dirSector = FAT1Sector + sectsPerFat * (formatBuf.bootRec.fatBR.numberOfFats);
  dirSects = (Nat16)((((Nat32)dirEntries * DIR_ENTRY_SIZE) + bytesPerSect - 1)
					 / bytesPerSect);
  dataSector = dirSector + dirSects;
  dataClusters = (sectors - dataSector)/(Nat16)sectorsPerCluster;

  xprintf("Sects = %lu, rsvd sects = %u, bytes/sect = %u\n",
		  sectors, rsvdSects, bytesPerSect);
  xprintf("Sects/cluster = %u, sects/FAT = %lu, Root dir entries = %u\n", 
		  (Nat16)sectorsPerCluster, sectsPerFat, dirEntries);
  xprintf("Start of FAT1 = %u, Start of FAT2 = %u, Start of directory = %u\n",
		  FAT1Sector, FAT2Sector, dirSector);
  xprintf("Data Clusters = %lu, Number Dir sectors = %u\n", dataClusters, dirSects);

  if (dataClusters >= 65525)
  {
	xputs("\n\nThis looks like a FAT32 volume.	No need to reformat.  Exiting\n");
	return;
  }

  xputs("\nNew sectors/cluster (suggest 32):  ");

  if (((val = getDecInput(buffer)) < 2) || (val > 128))
  {
	xputs("Bad value\n");
	return;
  }

  sectorsPerCluster = (Byte)val;

  xputs("\nNew #Root Directory Entries (suggest 4096):	");

  if ((val = getDecInput(buffer)) < 256)
  {
	xputs("Bad value\n");
	return;
  }

  dirEntries = min(val, 16384);

  dirSects = (Nat16)((((Nat32)dirEntries * DIR_ENTRY_SIZE) + bytesPerSect - 1)
					 / bytesPerSect);

  FATEntriesPerSect = bytesPerSect / sizeof(Nat16);
  dataClusters = (sectors - dirSects)/sectorsPerCluster;
  if (dataClusters > 65535L)
  {
	xprintf("\n\nCluster size too small\n");
	return;
  }

  dataClusters = (sectors - dirSects)/sectorsPerCluster;
  sectsPerFat = (dataClusters + FATEntriesPerSect - 1) / FATEntriesPerSect;
  dataClusters = (sectors - dirSects - 2 * sectsPerFat)/sectorsPerCluster;
  sectsPerFat = (dataClusters + FATEntriesPerSect - 1) / FATEntriesPerSect;
  FAT1Sector = rsvdSects;
  FAT2Sector = FAT1Sector + sectsPerFat;
  dirSector = FAT2Sector + sectsPerFat;
  dataSector = dirSector + dirSects;
  dataClusters = (sectors - dataSector)/(Nat16)sectorsPerCluster;

  xprintf("\n\nSects = %lu, rsvd sects = %u, bytes/sect = %u\n",
		  sectors, rsvdSects, bytesPerSect);
  xprintf("Sects/cluster = %u, sects/FAT = %lu, Root dir entries = %u\n", 
		  (Nat16)sectorsPerCluster, sectsPerFat, dirEntries);
  xprintf("Start of FAT1 = %u, Start of FAT2 = %u, Start of directory = %u\n",
		  FAT1Sector, FAT2Sector, dirSector);
  xprintf("Data Clusters = %lu, Number Dir sectors = %u\n", dataClusters, dirSects);

  formatBuf.bootRec.fatBR.sectorsPerCluster = sectorsPerCluster;
  formatBuf.bootRec.fatBR.sectorsPerFat = swapWord(sectsPerFat);
  formatBuf.bootRec.fatBR.numberOfFats = 2;
  formatBuf.bootRec.fatBR.rootDirEntries_hi	 = (Byte)(dirEntries >> 8);
  formatBuf.bootRec.fatBR.rootDirEntries_low = (Byte)(dirEntries & 0xff);
  DSDWriteSectors(DRIVE, 0L, &formatBuf, 1);

  memset(&formatBuf, 0, sizeof(formatBuf));
  formatBuf.raw[0] = 0xf8;
  formatBuf.raw[1] = 0xff;
  formatBuf.raw[2] = 0xff;
  formatBuf.raw[3] = 0xff;
  DSDWriteSectors(DRIVE, (Nat32)(FAT1Sector), &formatBuf, 1);
  DSDWriteSectors(DRIVE, (Nat32)(FAT2Sector), &formatBuf, 1);

  memset(&formatBuf, 0, sizeof(formatBuf));
  for (i = 1; i < sectsPerFat; i++ )
  {
	DSDWriteSectors(DRIVE, (Nat32)(FAT1Sector + i), &formatBuf, 1);
	DSDWriteSectors(DRIVE, (Nat32)(FAT2Sector + i), &formatBuf, 1);
  }

  for (i = 0; i < dirSects; i++ )
	DSDWriteSectors(DRIVE, (Nat32)(dirSector + i), &formatBuf, 1);

} /* formatDrive() */

