#include <nuttx/wireless/ieee80211/mmc_sdio.h>
#include <debug.h>
#include <errno.h>

#include <nuttx/arch.h>

#define SDIO_CMD53_TIMEOUT_MS 100
#define SDIO_IDLE_DELAY_MS    50

struct __attribute__((packed)) sdio_cmd52 {
    uint32_t write_data       : 8;
    uint32_t reserved_8       : 1;
    uint32_t register_address : 17;
    uint32_t reserved_26      : 1;
    uint32_t raw_flag         : 1;
    uint32_t function_number  : 3;
    uint32_t rw_flag          : 1;
};

struct __attribute__((packed)) sdio_cmd53 {
    uint32_t byte_block_count : 9;
    uint32_t register_address : 17;
    uint32_t op_code          : 1;
    uint32_t block_mode       : 1;
    uint32_t function_number  : 3;
    uint32_t rw_flag          : 1;
};

struct __attribute__((packed)) sdio_resp_R5 {
    uint32_t data             : 8;
    struct {
        uint32_t out_of_range     : 1;
        uint32_t function_number  : 1;
        uint32_t rfu              : 1;
        uint32_t error            : 1;
        uint32_t io_current_state : 2;
        uint32_t illegal_command  : 1;
        uint32_t com_crc_error    : 1;
    } flags;
    uint32_t reserved_16      : 16;
};

union sdio_cmd5x {
    uint32_t value;
    struct sdio_cmd52 cmd52;
    struct sdio_cmd53 cmd53;
};

int sdio_sendcmdpoll(FAR struct sdio_dev_s *dev, uint32_t cmd, uint32_t arg)
{
  int ret;

  /* Send the command */

  ret = SDIO_SENDCMD(dev, cmd, arg);
  if (ret == OK)
    {
      /* Then poll-wait until the response is available */

      ret = SDIO_WAITRESPONSE(dev, cmd);
      if (ret != OK)
        {
          _err("ERROR: Wait for response to cmd: %08x failed: %d\n",
               cmd, ret);
        }
    }

  return ret;
}

int sdio_io_rw_direct(FAR struct sdio_dev_s *dev, bool write,
                      uint8_t function, uint32_t address,
                      uint8_t inb, uint8_t* outb)
{
    union sdio_cmd5x arg;
    struct sdio_resp_R5 resp;
    int ret;

    /* Setup CMD52 argument */

    arg.cmd52.write_data       = inb;
    arg.cmd52.register_address = address & 0x1ffff;
    arg.cmd52.raw_flag         = (write && outb);
    arg.cmd52.function_number  = function & 7;
    arg.cmd52.rw_flag          = write;

    /* Send CMD52 command */

    sdio_sendcmdpoll(dev, SDIO_ACMD52, arg.value);
    ret = SDIO_RECVR5(dev, SDIO_ACMD52, (uint32_t*)&resp);

    if (ret != OK)
      {
        _err("ERROR: SDIO_RECVR5 failed %d\n", ret);
        return ret;
      }

    /* Check for errors */

    if (resp.flags.error)
      {
        return -EIO;
      }
    if (resp.flags.function_number || resp.flags.out_of_range)
      {
        return -EINVAL;
      }

    /* Write output byte */

    if (outb)
      {
        *outb = resp.data & 0xff;
      }

    return OK;
}

int sdio_io_rw_extended(FAR struct sdio_dev_s *dev, bool write,
                        uint8_t function, uint32_t address,
                        bool inc_addr, uint8_t *buf,
                        unsigned int blocklen, unsigned int nblocks)
{
    union sdio_cmd5x arg;
    struct sdio_resp_R5 resp;
    int ret;
    sdio_eventset_t wkupevent;

    /* Setup CMD53 argument */

    arg.cmd53.byte_block_count = blocklen;
    arg.cmd53.register_address = address & 0x1ffff;
    arg.cmd53.op_code          = inc_addr;
    arg.cmd53.function_number  = function & 7;
    arg.cmd53.rw_flag          = write;

    if (nblocks <= 1 && blocklen < 512)
      {
        /* Use byte mode */

        _info("byte mode\n");
        arg.cmd53.block_mode = 0;
        nblocks = 1;
      }
    else
      {
        /* Use block mode */

        arg.cmd53.block_mode = 1;
      }

    /* Send CMD53 command */

    SDIO_BLOCKSETUP(dev, blocklen, nblocks);
    SDIO_WAITENABLE(dev,
            SDIOWAIT_TRANSFERDONE | SDIOWAIT_TIMEOUT | SDIOWAIT_ERROR);

    if (write)
      {
        sdio_sendcmdpoll(dev, SDIO_ACMD53, (uint32_t)arg.value);
        ret = SDIO_RECVR5(dev, SDIO_ACMD53, (uint32_t*)&resp);

        SDIO_SENDSETUP(dev, buf, blocklen * nblocks);
        wkupevent = SDIO_EVENTWAIT(dev, SDIO_CMD53_TIMEOUT_MS);
      }
    else
      {
        _info("prep read %d\n", blocklen * nblocks);
        SDIO_RECVSETUP(dev, buf, blocklen * nblocks);
        SDIO_SENDCMD(dev, SDIO_ACMD53, (uint32_t)arg.value);

        wkupevent = SDIO_EVENTWAIT(dev, SDIO_CMD53_TIMEOUT_MS);
        ret = SDIO_RECVR5(dev, SDIO_ACMD53, (uint32_t*)&resp);
      }

    if (ret != OK)
      {
        _err("ERROR: SDIO_RECVR5 failed %d\n", ret);
        return ret;
      }

    /* Check for errors */

    if (wkupevent & SDIOWAIT_TIMEOUT)
      {
        _err("timeout\n");
        return -ETIMEDOUT;
      }
    if (resp.flags.error || (wkupevent & SDIOWAIT_ERROR))
      {
        _err("error 1\n");
        return -EIO;
      }
    if (resp.flags.function_number || resp.flags.out_of_range)
      {
        _err("error 2\n");
        return -EINVAL;
      }

    return OK;
}

int sdio_set_wide_bus(struct sdio_dev_s *dev)
{
  int ret;
  uint8_t value;

  /* Read Bus Interface Control register */

  ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_BUS_IF, 0, &value);
  if (ret != OK)
    {
      return ret;
    }

  /* Set 4 bits bus width setting */

  value &= ~SDIO_CCCR_BUS_IF_WIDTH_MASK;
  value |= SDIO_CCCR_BUS_IF_4_BITS;

  ret = sdio_io_rw_direct(dev, true, 0, SDIO_CCCR_BUS_IF, value, NULL);
  if (ret != OK)
    {
  	  return ret;
    }
  
  SDIO_WIDEBUS(dev, true);
  return OK;
}

int sdio_probe(FAR struct sdio_dev_s *dev)
{
  int ret;
  uint32_t data = 0;

  /* Set device state from reset to idle */

  sdio_sendcmdpoll(dev, MMCSD_CMD0, 0);
  up_mdelay(SDIO_IDLE_DELAY_MS);

  /* Device is SDIO card compatible so we can send CMD5 instead of ACMD41 */

  sdio_sendcmdpoll(dev, SDIO_CMD5, 0);

  /* Receive R4 response */

  ret = SDIO_RECVR4(dev, SDIO_CMD5, &data);
  if (ret != OK)
    {
      return ret;
    }

  /* Device is in Card Identification Mode, request device RCA */

  sdio_sendcmdpoll(dev, SD_CMD3, 0);

  ret = SDIO_RECVR6(dev, SD_CMD3, &data);
    if (ret != OK)
    {
        _err("ERROR: RCA request failed: %d\n", ret);
        return ret;
    }

    _info("rca is %x\n", data >> 16);

  /* Send CMD7 with the argument == RCA in order to select the card
   * and put it in Transfer State */

  sdio_sendcmdpoll(dev, MMCSD_CMD7S, data & 0xffff0000);

  ret = SDIO_RECVR1(dev, MMCSD_CMD7S, &data);
  if (ret != OK)
    {
      _err("ERROR: card selection failed: %d\n", ret);
      return ret;
    }

  /*  Configure 4 bits bus width */

  ret = sdio_set_wide_bus(dev);
  if (ret != OK)
    {
      return ret;
    }

  return OK;
}

int sdio_set_blocksize(FAR struct sdio_dev_s *dev, uint8_t function,
                       uint16_t blocksize)
{
  int ret;

  ret = sdio_io_rw_direct(dev, true, 0,
                  (function << SDIO_FBR_SHIFT) + SDIO_CCCR_FN0_BLKSIZE_0,
                  blocksize & 0xff, NULL);
  if (ret != OK)
    {
      return ret;
    }

  ret = sdio_io_rw_direct(dev, true, 0,
                  (function << SDIO_FBR_SHIFT) + SDIO_CCCR_FN0_BLKSIZE_1,
                  (blocksize >> 8), NULL);

  if (ret != OK)
    {
      return ret;
    }

  return OK;
}

int sdio_enable_function(FAR struct sdio_dev_s *dev, uint8_t function)
{
  int ret;
  uint8_t value;

  /* Read current I/O Enable register */

  ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_IOEN, 0, &value);
  if (ret != OK)
    {
      return ret;
    }
 
  ret = sdio_io_rw_direct(dev, true, 0, SDIO_CCCR_IOEN, value | (1 << function), NULL);

  if (ret != OK)
    {
      return ret;
    }

  /* Wait 10ms for function to be enabled */

  int loops = 10;

  while (loops-- > 0)
    {
      up_mdelay(1);

      ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_IOEN, 0, &value);
      if (ret != OK)
        {
          return ret;
        }

      if (value & (1 << function))
        {
          /* Function enabled */

          _info("Function %d enabled\n", function);
          return OK;
        }
    }
  return -ETIMEDOUT;
}

int sdio_enable_interrupt(FAR struct sdio_dev_s *dev, uint8_t function)
{
  int ret;
  uint8_t value;

  /* Read current Int Enable register */

  ret = sdio_io_rw_direct(dev, false, 0, SDIO_CCCR_INTEN, 0, &value);
  if (ret != OK)
    {
      return ret;
    }

  return sdio_io_rw_direct(dev, true, 0, SDIO_CCCR_INTEN, value | (1 << function), NULL);
}