#include "project.h"


static enum
{
  STATE_SYNC = 0,
  STATE_ADDR = 1,
  STATE_ENDPOINT = 2,
  STATE_LEN = 3,
  STATE_DATA = 4,
} state = STATE_SYNC;


static uint8_t buf[16];
static int addr;
static int endpoint;
static unsigned int len;
static unsigned int ptr;

void
kvm_dispatch (void)
{
#if 0
  printf
    ("Addr %d, Write to ep %x, %d bytes %02x %02x %02x %02x ...\r\n",
     addr, endpoint, len, buf[0], buf[1], buf[2], buf[3]);
#endif

  if (len) {
    if (endpoint==0x81)
	led_set(GPIO8);
    else 
	led_set(GPIO9);

    if (endpoint==0x84) {
        SET_REG(USB_CNTR_REG, USB_CNTR_RESETM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM | USB_CNTR_RESUME);
    {
  int i;
  for (i = 0; i < 0x80000; i++)
    __asm__ ("nop");
  }

    SET_REG(USB_CNTR_REG, USB_CNTR_RESETM | USB_CNTR_CTRM | USB_CNTR_SUSPM | USB_CNTR_WKUPM);
    } else {
     usbd_ep_write_packet (usbd_dev, endpoint, buf, len);
    }
  }
}

void
kvm_recv (uint8_t d)
{
//  printf ("S%d V0x%02x\r\n", state, d);

  if (state == STATE_ADDR)
    {
      addr = d;
      usart2_queue (d - 1);
      usart3_queue (d - 3);
      state = STATE_ENDPOINT;
      return;
    }
  else
    {
      usart2_queue (d);
      usart3_queue (d);
    }


  switch (state)
    {
    case STATE_SYNC:
      if (d != 0x5a)
        break;
      state = STATE_ADDR;
      break;
    case STATE_ENDPOINT:
      endpoint = d;
      if ((endpoint < 0x81) || (endpoint > 0x83))
        {
          state = STATE_SYNC;
        }
      else
        {
          state = STATE_LEN;
        }
      break;
    case STATE_LEN:
      len = d;
      ptr = 0;
      if (len > sizeof (buf))
        {
          state = STATE_SYNC;
        }
      else
        {
          state = STATE_DATA;
        }
      break;
    case STATE_DATA:
      buf[ptr++] = d;
      if (ptr >= len)
        {
          if (addr==BUS_ID)
            kvm_dispatch ();
          state = STATE_SYNC;
        }
      break;
    default:
	break;
    }
}