/* 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 <ringbuf.h>
#include <imem.h>
#include <vsos.h>
#include <consolestate.h>


int hexMode = 0;
int slowTx = 0;

/* If one below a power of two, memory usage efficiency is best */
#define UARTMUX_RX_BUFFER_SIZE (4096-1)
/* If one below a power of two, memory usage efficiency is best */
#define UARTMUX_TX_BUFFER_SIZE (256-1)
__mem_y char __align uartMuxRxBuffer[UARTMUX_RX_BUFFER_SIZE];
__mem_y char __align uartMuxTxBuffer[UARTMUX_TX_BUFFER_SIZE];

struct UartMux {
  FILE *origIn, *origOut;
  u_int16 origIntEna1Mask;
  u_int32 origRxVec, origTxVec;
  __mem_y char *rxRd, *rxWr, *txRd, *txWr;
} uart = {
  0, 0, 0, 0, 0, uartMuxRxBuffer, uartMuxRxBuffer,
  uartMuxTxBuffer, uartMuxTxBuffer
};



extern u_int16 IntUartMuxRxVector;
extern u_int16 IntUartMuxTxVector;

void IntUartMuxTxC(void) {
  while (!(PERIP(UARTMUX_STATUS) & UART_ST_TXFULL) && uart.txWr != uart.txRd) {
    PERIP(UARTMUX_DATA) = *uart.txRd;
    uart.txRd = ringmodY(uart.txRd, MAKEMODF(UARTMUX_TX_BUFFER_SIZE));
  }
}

void IntUartMuxRxC(void) {
  *uart.rxWr = PERIP(UARTMUX_DATA);
  uart.rxWr = ringmodY(uart.rxWr, MAKEMODF(UARTMUX_RX_BUFFER_SIZE));
}



void UartMuxPutChar(register char c) {
  __mem_y char *nextTxWr =
    ringmodY(uart.txWr, MAKEMODF(UARTMUX_TX_BUFFER_SIZE));

  while (nextTxWr == uart.txRd) {
    Delay(1);
  }

  Disable();
  if (!(PERIP(UARTMUX_STATUS) & UART_ST_TXFULL) && uart.txWr == uart.txRd) {
    PERIP(UARTMUX_DATA) = c;
  } else {
    *uart.txWr = c;
    uart.txWr = nextTxWr;
  }
  Enable();
}


void SendChar(register int c) {
  if (slowTx) {
    static u_int32 lastTime = 0;
    static int longPause = 0;
    u_int32 now = ReadTimeCount();
    u_int32 needTime = longPause ? TICKS_PER_SEC*2 : TICKS_PER_SEC/10;
  
    if (now - lastTime > 5*TICKS_PER_SEC) {
      longPause = 1;
    } else {
      longPause = 0;
      if (now - lastTime < needTime) {
	//      printf("(%ld/%ld)", needTime-(now-lastTime), needTime);
	Delay(needTime - (now-lastTime));
      }
    }
    lastTime = ReadTimeCount();
  }
  if (hexMode) {
    printf("\ntx %02x ", c);
  }
  UartMuxPutChar(c);
}


char UartMuxGetChar(void) {
  char ch;
  while (uart.rxRd == uart.rxWr) {
    Delay(1);
  }
  ch = *uart.rxRd;
  uart.rxRd = ringmodY(uart.rxRd, MAKEMODF(UARTMUX_RX_BUFFER_SIZE));
  return ch;
}

extern SIMPLE_FILE uartMuxInFile;

IOCTL_RESULT UartMuxIoctl(register __i0 VO_FILE *self, s_int16 request, IOCTL_ARGUMENT arg) {
  if (request == IOCTL_TEST) {
    int res;
    if ((void *)self == &uartMuxInFile) {
      if ((res = uart.rxWr-uart.rxRd) < 0) res += UARTMUX_RX_BUFFER_SIZE;
    } else {
      if ((res = uart.txWr-uart.txRd) < 0) res += UARTMUX_TX_BUFFER_SIZE;
    }
    return res;
  }
  return S_ERROR;
};

u_int16 UartMuxWrite(register __i0 VO_FILE *self, void *buf, u_int16 sourceIndex, u_int16 bytes) {
  SendChar(*((u_int16 *)buf)); //Send character to raw UART port
  return 1;
};

u_int16 UartMuxRead(register __i0 VO_FILE *self, void *buf, u_int16 destinationIndex, u_int16 bytes) {
  char ch = UartMuxGetChar();
  /* Hack: Force vo_fgetc() to get 16-bit value in case UTF16 code doesn't
     fit into values 0..255 */
  if (destinationIndex == 1 && bytes == 1) {
    destinationIndex = 0;
    bytes = 2;
  } else {
    destinationIndex = bytes = 1;
  }
  MemCopyPackedBigEndian(buf, destinationIndex, &ch, destinationIndex, bytes);
  return 1;
};

extern u_int16 IntMUartMuxRxVector;
extern u_int16 IntUartMuxTxVector;

void UartMuxInit(char *parameters) {
  uart.origIntEna1Mask = PERIP(INT_ENABLE1_HP) | ~(INTF1_UARTMUX_TX|INTF1_UARTMUX_RX);
  uart.origRxVec = ReadIMem((0x20+INTV_UARTMUX_RX));
  uart.origTxVec = ReadIMem((0x20+INTV_UARTMUX_TX));
#if 0
  uart.origIn = vo_stdin;
  uart.origOut = vo_stdout;
#endif
}

// Library finalization code. This is called when DropLibrary is called
void UartMuxFini(void) {
  PERIP(INT_ENABLE1_HP) &= uart.origIntEna1Mask;
  WriteIMem((0x20+INTV_UARTMUX_RX), uart.origRxVec);
  WriteIMem((0x20+INTV_UARTMUX_TX), uart.origTxVec);
#if 0
  vo_stdin = uart.origIn;
  vo_stdout = uart.origOut;
#endif
}





const FILEOPS ConsoleInFileOperations = { NULL, NULL, UartMuxIoctl, UartMuxRead, UartMuxWrite };
//DLLENTRY(uartInFile)
SIMPLE_FILE uartMuxInFile = {__MASK_PRESENT | __MASK_OPEN | __MASK_READABLE | __MASK_WRITABLE | __MASK_FILE, NULL, &ConsoleInFileOperations};

const FILEOPS ConsoleOutFileOperations = { NULL, NULL, UartMuxIoctl, UartMuxRead, UartMuxWrite };
//DLLENTRY(uartOutFile)
SIMPLE_FILE uartMuxOutFile = {__MASK_PRESENT | __MASK_OPEN | __MASK_READABLE | __MASK_WRITABLE | __MASK_FILE, NULL, &ConsoleOutFileOperations};







#define MAX_ESC_SEQ_LEN 5

char escSeqBuf[MAX_ESC_SEQ_LEN];
int escSeqLen = 0;

struct EscSeq {
  int len;
  int retCode;
  char seq[MAX_ESC_SEQ_LEN];
} escSeq[] = {
  {3, -16, {0x1b, 0x4f, 0x51,   -1,   -1 }}, /*  0:  F2 */
  {3, -15, {0x1b, 0x4f, 0x52,   -1,   -1 }}, /*  1:  F3 */
  {3, -14, {0x1b, 0x4f, 0x53,   -1,   -1 }}, /*  2:  F4 */
  {5, -16, {0x1b, 0x5b, 0x31, 0x32, 0x7e }}, /*  3:  F2 */
  {5, -15, {0x1b, 0x5b, 0x31, 0x33, 0x7e }}, /*  3:  F3 */
  {5, -14, {0x1b, 0x5b, 0x31, 0x34, 0x7e }}, /*  3:  F4 */
  {5, -13, {0x1b, 0x5b, 0x31, 0x35, 0x7e }}, /*  3:  F5 */
  {5, -12, {0x1b, 0x5b, 0x31, 0x37, 0x7e }}, /*  4:  F6 */
  {5, -11, {0x1b, 0x5b, 0x31, 0x38, 0x7e }}, /*  5:  F7 */
  {5, -10, {0x1b, 0x5b, 0x31, 0x39, 0x7e }}, /*  6:  F8 */
  {5,  -9, {0x1b, 0x5b, 0x32, 0x30, 0x7e }}, /*  7:  F9 */
  {5,  -8, {0x1b, 0x5b, 0x32, 0x34, 0x7e }}, /*  8: F12 */
  {3,  -7, {0x1b, 0x5b, 0x41,   -1,   -1 }}, /*  9: CUp */
  {3,  -6, {0x1b, 0x5b, 0x42,   -1,   -1 }}, /* 10: CDn */
  {3,  -5, {0x1b, 0x5b, 0x43,   -1,   -1 }}, /* 11: CRg */
  {3,  -4, {0x1b, 0x5b, 0x44,   -1,   -1 }}, /* 12: CLf */
};

void PurgeEscBuf(register int dump) {
#if 0
  printf("*PurgeEscBuf %d*", escSeqLen);
#endif
  if (dump) {
    int i;
    for (i=0; i<escSeqLen; i++) {
      SendChar(escSeqBuf[i]);
    }
  }
  escSeqLen = 0;
}

int HandleEscBuf(register int c) {

 do_again:
  /* Do we collect data to out escape buffer? */
  if (escSeqLen || c == 0x1b) {
    int i, canBeAny = 0, match = -1;
    struct EscSeq *es = escSeq;
    escSeqBuf[escSeqLen++] = c;
    for (i=0; i<sizeof(escSeq)/sizeof(escSeq[0]); i++) {
      if (!memcmp(escSeqBuf, es->seq, escSeqLen)) {
	/* Our current escape sequency may be a part of a longer one. */
	canBeAny = 1;
	if (escSeqLen == es->len) {
	  /* Match: this is the escape sequence */
	  c = es->retCode;
	  PurgeEscBuf(0);
	  goto finally;
	}
      }
      es++;
    }
    /* If this escape sequence cannot be a part of any of the ones we
       recognize, dump it with the exception of the last, imcompatible
       character, that will be handled again. */
    if (!canBeAny/* || escSeqLen > MAX_ESC_SEQ_LEN*/) {
      escSeqLen--;
      PurgeEscBuf(1);
      /* After clearing buffer, we still have the current character
	 unhandled, and as it might be ESC, make a second round for c. */
      goto do_again;
    } else {
      c = -1; /* Buffer is being filled, no output yet */
    }
  }

 finally:
  return c;
}



ioresult main(char *parameters) {
  char *p = parameters;
  int nParam;
  int verbose = 1, setDivider = 1, i;
  int retCode = S_ERROR;
  u_int16 divider = 0;
  int quit = 0;
  u_int32 lastInputCharTime = 0;
  u_int16 origAppFlags = appFlags;

  appFlags |= APP_FLAG_NOBOOT;

  UartMuxInit(NULL);
  PurgeEscBuf(0);

#if 0
  divider = GetUartRegister(clockSpeed.cpuClkHz, 1500000);
  PERIP(UARTRV_DIV) = divider;
  PERIP(UARTRV_DATA) = 'a';
  while (ioctl(stdin, IOCTL_TEST, NULL) == 0) {
    if (PERIP(UARTRV_STATUS) & UART_ST_RXFULL) {
      printf("Got char 0x%x\n", PERIP(UARTRV_DATA));
    }
  }
#endif
  
  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 (!strcmp(p, "-a")) {
      printf("MUX VSDSP\n");
      PERIP(VSRV1_CF) |= VSRV1_CF_MUX;
    } else if (!strcmp(p, "-b")) {
      printf("MUX RV\n");
      PERIP(VSRV1_CF) &= ~VSRV1_CF_MUX;
    } else if (!strcmp(p, "-c")) {
      appFlags |= APP_FLAG_NOBOOT;
    } else if (!strcmp(p, "+c")) {
      appFlags &= ~APP_FLAG_NOBOOT;
    } else if (!strcmp(p, "-d")) {
      setDivider = 0;
    } else if (!strcmp(p, "-h")) {
      printf("Usage: Term [-v|+v|-a|-b|-s|+s|-h]\n"
	     "-v|+v\tVerbose on|off\n"
	     "-a|-b\tMux VSDSP|RV to UART\n"
	     "-s|+s\tSlow TX on|off\n"
	     "-c|+c\tDisable Ctrl-C reset on/off (default: on)\n"
	     "-d\tDon't set divider\n"
	     "-h\tShow this help\n"
	     "\nConsole keys:\n"
	     "\tF2 - MUX VSDSP\n"
	     "\tF3 - MUX RV\n"
	     "\tF4 - Exit\n"
	     "\tF5 - Toggle Hex Mode\n"
	     "\tF6 - Toggle Slow TX mode\n"
	     );
      goto finally_ok;
    } else {
      printf("E: Unknown parameter \"%s\".\n", p);
      goto finally;
    }
    p += strlen(p)+1;
  }

  /* in */
#if 1
  WriteIMem((0x20+INTV_UARTMUX_RX), ReadIMem((u_int16)(&IntUartMuxRxVector)));
  PERIP(INT_ENABLE1_HP) |= INTF1_UARTMUX_RX;
  //vo_stdin = &uartMuxInFile;
#endif
  
  /* out */
#if 1
  WriteIMem((0x20+INTV_UARTMUX_TX), ReadIMem((u_int16)(&IntUartMuxTxVector)));
  PERIP(INT_ENABLE1_HP) |= INTF1_UARTMUX_TX;
  //vo_stdout = &uartMuxOutFile;
#endif

  
  divider = GetUartRegister(clockSpeed.cpuClkHz, 4000000);
  divider = 12;
  printf("Old divider = 0x%04x\n", PERIP(UARTMUX_DIV));
  if (setDivider) {
    PERIP(UARTMUX_DIV) = divider;
    printf("New divider = 0x%04x\n", divider);
  }
#if 0
  PERIP(VSRV1_CF) |= VSRV1_CF_MUX;
#endif
  
  while (!quit) {
#if 0
    while (PERIP(UARTMUX_STATUS) & UART_ST_RXFULL) {
      int c = PERIP(UARTMUX_DATA);
      if (hexMode) {
	printf("MUXRX %02x\n", c);
      } else {
	printf("%c", c);
      }
    }
#else
    int n;
    while ((n = UartMuxIoctl(&uartMuxInFile, IOCTL_TEST, NULL) > 0)) {
      int i;
      for (i=0; i<n; i++) {
	printf("%c", UartMuxGetChar());
      }
    }
#endif
      
#if 1
    if (ReadTimeCount() - lastInputCharTime > TICKS_PER_SEC && escSeqLen) {
      PurgeEscBuf(1);
    }
    if (ioctl(stdin, IOCTL_TEST, 0) > 0) {
      int c = fgetc(stdin);
      lastInputCharTime = ReadTimeCount();
      c = HandleEscBuf(c);
      switch (c) {
      case -14:
	printf("*EXIT TERMINAL*\n");
	quit = 1;
	break;
      case -16:
	printf("MUX VSDSP\n");
	PERIP(VSRV1_CF) |= VSRV1_CF_MUX;
	break;
      case -15:
	printf("MUX RV\n");
	PERIP(VSRV1_CF) &= ~VSRV1_CF_MUX;
	break;
      case -13:
	hexMode = 1-hexMode;
	printf("hexMode = %d\n", hexMode);
	break;
      case -12:
	slowTx = 1-slowTx;
	printf("slowTx = %d\n", slowTx);
	break;
      case -7:
	SendChar(0x10); /* CUp */
	break;
      case -6:
	SendChar(0x0e); /* CDn */
	break;
      case -5:
	SendChar(0x06); /* CRg */
	break;
      case -4:
	SendChar(0x02); /* CLf */
	break;
#if 0
      case 'V'-('A'-1):
	PERIP(UARTMUX_DIV) = divider+1;
	printf("Slower speed 0x%04x\n", PERIP(UARTMUX_DIV));
	break;
      case 'W'-('A'-1):
	PERIP(UARTMUX_DIV) = divider;
	printf("Nominal speed 0x%04x\n", PERIP(UARTMUX_DIV));
	break;
      case 'U'-('A'-1):
	PERIP(UARTMUX_DIV) = divider-1;
	printf("Slower speed 0x%04x\n", PERIP(UARTMUX_DIV));
	break;
#endif
      case -1:
	/* Esc buf handled the character already. */
	break;
      default:
	SendChar(c);
	break;
      }
    }
#endif
#if 0
    if (PERIP(UARTRV_STATUS) & UART_ST_RXFULL) {
      int c = PERIP(UARTRV_DATA);
      if (hexMode) {
	printf("RVRX! %02x\n", c);
      } else {
	printf("%c", c);
      }
    }
#endif
#if 0
    if (PERIP(UARTMUX_STATUS) & UART_ST_FRAMERR) {
      static int n = 0;
      printf("MUX FRAME ERROR %d!\n", n++);
      PERIP(UARTMUX_STATUS) &= ~UART_ST_FRAMERR;
    }
#endif
#if 0
    printf("%04x %04x\n", PERIP(UARTMUX_STATUS), PERIP(UARTRV_STATUS));
#endif
  }

 finally_ok:
  retCode = S_OK;
 finally:
  UartMuxFini();

  appFlags = origAppFlags;
  return retCode;
}

