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

// Revision-specific code

static uint8_t dac_addr_revabc[] = {
  I2C_ADDR_IOA_DAC_REVABC012,
  // (or I2C_ADDR_IOA_DAC_REVC3)
  I2C_ADDR_IOB_DAC,
};

static bool set_vsupply_revabc(uint8_t chan)
{
  // Offset 1650, slope -15.2, 0x1000/15.2 = 269
  // The DAC has a 12-bit code word, so we only shift back by 8
  uint16_t codeword = (254 << 4) - ((((mgmt_req.vsupply.value[chan] - 1650) >> 4) * 269) >> 4);
  i2c_sequence sequence[] = {
    I2C_WRITE(dac_addr_revabc[chan]),
    I2C_IMM16BE(codeword),
    I2C_DONE(),
  };
  return i2c_execute(sequence);
}

static bool get_vsupply_revabc(uint8_t chan)
{
  static __xdata uint16_t codeword;
  i2c_sequence sequence[] = {
    I2C_READ(dac_addr_revabc[chan]),
    I2C_PTR16LE(&codeword),
    I2C_DONE(),
  };
  if (!i2c_execute(sequence))
    return false;
  // See explanation in `set_vsupply_revabc()`.
  mgmt_rsp.vsupply.value[chan] = 1650 + (255 - (bswap16(codeword) >> 4)) * 152 / 10;
  return true;
}

static bool set_vsupply_revd(uint8_t chan)
{
  return false;
}

static bool get_vsupply_revd(uint8_t chan)
{
  return false;
}

// Revision-generic code

const uint8_t power_en_bit[] = {
  1<<PIND_EN_VA,
  1<<PIND_EN_VB,
  1<<PIND_EN_VC_REVD,
  1<<PIND_EN_VD_REVD,
};

struct power_config {
  uint8_t valid_mask;
  uint16_t vsupply_min;
  uint16_t vsupply_max;
  bool (*set_vsupply)(uint8_t chan);
  bool (*get_vsupply)(uint8_t chan);
};

const struct power_config power_config_revabc = {
  .valid_mask     = 0b11,
  .vsupply_min    = 1650,
  .vsupply_max    = 5500,
  .set_vsupply    = set_vsupply_revabc,
  .get_vsupply    = get_vsupply_revabc,
};

const struct power_config power_config_revd = {
  .valid_mask     = 0b1111,
  .vsupply_min    =  900,
  .vsupply_max    = 5500,
  .set_vsupply    = set_vsupply_revd,
  .get_vsupply    = get_vsupply_revd,
};

static const struct power_config *power_config;

void power_init()
{
  if (glasgow_config.revision == GLASGOW_REV_D0) {
    power_config = &power_config_revd;
    // Configure enables.
    IO_EN_VA = IO_EN_VB = IO_EN_VC_REVD = IO_EN_VD_REVD = 0;
    OED |= (1<<PIND_EN_VA)|(1<<PIND_EN_VB)|(1<<PIND_EN_VC_REVD)|(1<<PIND_EN_VD_REVD);
    // TODO: configure DAC
  } else {
    power_config = &power_config_revabc;
    // Configure enables.
    IO_EN_VA = IO_EN_VB = 0;
    OED |= (1<<PIND_EN_VA)|(1<<PIND_EN_VB);
    if (glasgow_config.revision >= GLASGOW_REV_C3) {
      // revC3 and later has the port A DAC strapped to a different address because
      // the address on previous revisions conflicts with SMBus Alert Response address.
      dac_addr_revabc[0] = I2C_ADDR_IOA_DAC_REVC3;
    }
    if (glasgow_config.revision <= GLASGOW_REV_B) {
      // Strap FXMA outputs as enabled.
      IO_OEQ_N_REVAB = 1;
      OED |= (1<<PIND_OEQ_N_REVAB);
    }
  }
}

bool power_mgmt_set_vsupply()
{
  // Validate request.
  if (mgmt_req.vsupply.mask & ~power_config->valid_mask)
    return false;
  for (uint8_t chan = 0; chan < ARRAYSIZE(mgmt_req.vsupply.value); chan++) {
    uint16_t millivolts = mgmt_req.vsupply.value[chan];
    // Check against physical limits.
    if (!(millivolts == 0 ||
          millivolts >= power_config->vsupply_min &&
          millivolts <= power_config->vsupply_max))
      return false;
    // TODO: Check against programmed limits.
  }

  // Apply request.
  for (uint8_t chan = 0; chan < ARRAYSIZE(mgmt_req.vsupply.value); chan++) {
    if (mgmt_req.vsupply.mask & (1<<chan)) {
      uint16_t millivolts = mgmt_req.vsupply.value[chan];
      if (millivolts == 0)
        IOD &= ~power_en_bit[chan];
      if (!power_config->set_vsupply(chan))
        return false;
      if (millivolts != 0)
        IOD |= power_en_bit[chan];
    }
  }

  return true;
}

bool power_mgmt_get_vsupply()
{
  mgmt_rsp.vsupply.mask = power_config->valid_mask;
  for (uint8_t chan = 0; chan < ARRAYSIZE(mgmt_req.vsupply.value); chan++) {
    if (mgmt_rsp.vsupply.mask & (1<<chan)) {
      if (IOD & power_en_bit[chan]) {
        if (!power_config->get_vsupply(chan))
          return false;
      } else {
        mgmt_rsp.vsupply.value[chan] = 0;
      }
    }
  }

  return true;
}