#include <fx2regs.h>
#include <fx2delay.h>
#include <fx2lib.h>
#include "glasgow.h"

void mgmt_init() {
  SYNCDELAY;
  EP1OUTCFG = _VALID|_TYPE1; // EP1OUT BULK
  SYNCDELAY;
  EP1INCFG  = _VALID|_TYPE1; // EP1IN  BULK
  SYNCDELAY;
  EP1OUTBC  = 0;             // arm EP1OUT
  SYNCDELAY;
  EP1INCS   = _BUSY;         // force disarm EP1IN
  SYNCDELAY;
}

void mgmt_poll() {
  if (EP01STAT & (_EP1OUTBSY|_EP1INBSY))
    return; // EP1OUT empty or EP1IN FULL
  if (EP1OUTBC < 2)
    goto error_pkt;

  xmemclr(&mgmt_rsp, sizeof(mgmt_rsp));

  uint8_t mgmt_req_len = EP1OUTBC - 2, mgmt_rsp_len = 0;
  switch (mgmt_req.opcode) {
  case MGMT_OP_ECHO:
    if (mgmt_req_len != sizeof(mgmt_req.echo))
      goto error_pkt;
    mgmt_rsp.opcode = MGMT_OP_ECHO;
    mgmt_rsp.echo.value = mgmt_req.echo.value;
    mgmt_rsp_len = sizeof(mgmt_rsp.echo);
    goto send_pkt;

  case MGMT_OP_SET_VSUPPLY:
    if (mgmt_req_len != sizeof(mgmt_req.vsupply))
      goto error_pkt;
    mgmt_rsp.opcode = MGMT_OP_EMPTY;
    if (power_mgmt_set_vsupply()) goto send_pkt; else goto error_pkt;
    goto send_pkt;

  case MGMT_OP_GET_VSUPPLY:
    if (mgmt_req_len != 0)
      goto error_pkt;
    mgmt_rsp.opcode = MGMT_OP_GET_VSUPPLY;
    mgmt_rsp_len = sizeof(mgmt_req.vsupply);
    if (power_mgmt_get_vsupply()) goto send_pkt; else goto error_pkt;

  default:
    // error
  }

error_pkt:
  mgmt_rsp.opcode = MGMT_OP_ERROR;
  mgmt_rsp_len = 0;
send_pkt:
  mgmt_rsp.serial = mgmt_req.serial;
  SYNCDELAY;
  EP1INBC = 2 + mgmt_rsp_len;
ack_ep0:
  SYNCDELAY;
  EP1OUTBC = 0;
}