#include <cstdint>
#include <errno.h>
#include <cstdio>
#include <vector>

#include "exceptions.hh"
#include "sleep.hh"
#include "globals.hh"
#include "drv_omap35x_gpt.hh"
#include "omap35x_intc.hh"
#include "omap35x_prcm.hh"
#include "drv_omap35x_sdmmc.hh"

class commanderror : public ex::error {
public:
  commanderror(uint32_t stat) : ex::error(EIO), stat(stat)  {
  }

  uint32_t stat;
};

class dataerror : public ex::error {
public:
  dataerror(uint32_t stat) : ex::error(EIO), stat(stat)  {
  }

  uint32_t stat;
};

#define ACMD(x) ((((x)) | 0x100))

typedef union {
  uint32_t data[4];
  struct {
    unsigned :8;
    unsigned mdt_m:4;
    unsigned mdt_y:8;
    unsigned :4;
    unsigned psn:32;
    unsigned prv:8; // BCD coded
    char pnm[5];
    char oid[2];
    unsigned mid:8;
  } __attribute__((packed)) bits;
} CID_reg;

typedef union {
  uint32_t data[4];
  struct {
    unsigned :10;
    unsigned file_format:2;
    unsigned tmp_write_protect:1;
    unsigned perm_write_protect:1;
    unsigned copy:1;
    unsigned file_format_grp:1;
    unsigned :5;
    unsigned write_bl_partial:1;
    unsigned write_bl_len:4;
    unsigned r2w_factor:3;
    unsigned :2;
    unsigned wp_grp_enable:1;
    unsigned wp_grp_size:7;
    unsigned sector_size:7;
    unsigned erase_blk_en:1;
    unsigned c_size_mult:3;
    unsigned vdd_w_curr_max:3;
    unsigned vdd_w_curr_min:3;
    unsigned vdd_r_curr_max:3;
    unsigned vdd_r_curr_min:3;
    unsigned c_size:12;
    unsigned :2;
    unsigned dsr_imp:1;
    unsigned read_blk_misalign:1;
    unsigned write_blk_misalign:1;
    unsigned read_bl_partial:1;
    unsigned read_bl_len:4;
    unsigned ccc:12;
    unsigned tran_speed:8;
    unsigned nsac:8;
    unsigned taac:8;
    unsigned :6;
    unsigned csd_structure:2;
  } __attribute__((packed)) v1;
  struct {
    unsigned :12;
    unsigned tmp_write_protect:1;
    unsigned perm_write_protect:1;
    unsigned copy:1;
    unsigned :32;
    unsigned :1;
    unsigned c_size:22;
    unsigned :6;
    unsigned dsr_imp:1;
    unsigned :7;
    unsigned ccc:12;
    unsigned :30;
    unsigned csd_structure:2;
  } __attribute__((packed)) v2;
} CSD_reg;

#define BCDDEC(x) (((((x))&0xf) + (((((x))>>4)&0xf)*10)))

OMAP35x_SDMMC::OMAP35x_SDMMC(uintptr_t base, unsigned irq, unsigned drq_rx, unsigned drq_tx)
  : base_{base}, irq_(irq), drq_rx_(drq_rx), drq_tx_(drq_tx), rca_(0), intSem_(0) {
    printf("OMAP35x SD/MMC controller\n");
  // Reset the controller
  r_sysconfig() |= (1<<1);
  while(!(r_sysstatus() & (1<<0)));
  
  r_sysctl() |= (1<<24); // Software reset all
  while(r_sysctl() & (1<<24));

  // Set capabilities
  uint32_t capa = (1<<26);
  if(base_.get_phys() == 0x4809c000) { // MMC 1
    printf("MMCHS1, activating 3.0V support\n");
    capa |= (1<<25);
  }
  r_capa() |= capa;

  // Install interrupt handler
  global::intc->register_handler(irq_, std::bind(&OMAP35x_SDMMC::int_handler, this), 0);
  global::intc->enable_int(irq_);

  // Configure
  r_hctl() = (r_hctl() & ~(0x7<<9)) | (0x6<<9); // 3.0V mode
  r_hctl() |= (1<<8); // power on
  while(!(r_hctl() & (1<<8)));

  // TODO: padconf

  changeClock(0x3ff);

  r_ie() = (1<<28) | (1<<22) | (1<<21) | (1<<20) |(1<<19) | (1<<18) | (1<<17) | (1<<16) | (1<<5) | (1<<1) | (1<<0); 

  // Card detect
  r_con() |= (1<<1); // Send initialization stream
  r_cmd() = 0x0; // Dummy command

  usleep(1000);

  r_stat() |= (1<<0);
  r_con() &= ~(1<<1); // End initialization stream
  r_stat() = 0xffffffff;

  // Set clock to ~400 kHz
  changeClock(240);

  cmd_reset();

  r_ise() = (1<<28) | (1<<22) | (1<<21) // | (1<<20)
    |(1<<19) | (1<<18) | (1<<17) | (1<<16) // | (1<<5) | (1<<1) 
    | (1<<0);

  printf("Probing SD/MMC card...\n");
  try {
    // Send CMD0
    command(0, 0);

    // Here we could test for SDIO (send CMD5), but we don't support SDIO (yet)
    bool sdv2 = false;
    uint32_t resp;

    try {
      resp = command_r1(8, 0x000001a5);
      if((resp&0xfff) != 0x1a5) {
    	printf("CMD8 unexpected response: %.8lx\n", resp);
    	throw ex::error{EIO};
      } else
    	sdv2 = true;
    } catch(commanderror &ex) {
      printf("CMD8 failed: %.8lx\n", ex.stat);
    } catch(ex::timeout &ex) {
      printf("CMD8 timed out\n");
      cmd_reset();
    }
 
    bool sd = false;
    uint32_t startTicks = global::ticktimer->getTicks(), endTicks;
    uint32_t arg = (sdv2?0x50000000:0);

    while((endTicks = global::ticktimer->getTicks()) < (startTicks+1000/TICKTIMER_MS)) { // Try for 1s
      try {
	resp = command_r1(ACMD(41), arg);
	arg |= (0x1ff<<15); // Set OCR bits after first ACMD41
      } catch (ex::timeout &ex) {
	break; // On command timeout we don't have to continue trying
      }
      if(resp & (1<<31)) {
	printf("ACMD41 resp: %.8lx after %lu ms\n", resp, (endTicks-startTicks)*TICKTIMER_MS);
	sd = true;
	break;
      }
    }
 
    if(!sd) { // Try MMC init
      assert(!sdv2);
      arg = 0;
      r_con() |= (1<<0); // Open drain
      startTicks = global::ticktimer->getTicks();
      while((endTicks = global::ticktimer->getTicks()) < startTicks+1000/TICKTIMER_MS) {
	try {
	  resp = command_r1(1, 0);
	  arg |= (0x1ff<<15); // Set OCR bits after first CMD1
	} catch (ex::timeout &ex) {
	  // On command timeout we don't have to continue trying
	  // Unknown/Unsupported card type
	  throw ex::error{EIO};
	}
	if(resp & (1<<31)) {
	  printf("CMD1 resp: %.8lx after %lu ms\n", resp, (endTicks-startTicks)*TICKTIMER_MS);
	  break;
	}
      }
    }

    std::array<uint32_t, 4> resp4 = command_r2(2, 0);
    CID_reg cid;
    for(int i = 0;i <4;++i)
      cid.data[i] = resp4[i];
    printf(" %c%c %c%c%c%c%c Rev %hhu Serial# %.8x Date %u/%u\n",
	   cid.bits.oid[1], cid.bits.oid[0],
	   cid.bits.pnm[4],  cid.bits.pnm[3],  cid.bits.pnm[2],  cid.bits.pnm[1],  cid.bits.pnm[0],
	   cid.bits.prv, cid.bits.psn, cid.bits.mdt_m, 2000+cid.bits.mdt_y);

    resp = command_r1(3, 0);
 
    resp &= 0xffff0000;
    rca_ = resp>>16;

    r_con() &= ~(1<<0); // No more open drain

    resp4 = command_r2(9, rca_<<16);
    printf("CSD: %.8lx %.8lx %.8lx %.8lx\n",
     	   resp4[3], resp4[2], resp4[1], resp4[0]);

    CSD_reg csd;
    for(int i = 0;i <4;++i)
      csd.data[i] = resp4[i];

    if((csd.v1.csd_structure == 1) && sdv2) {
      blockSize_ = 512;
      sectorSize_ = 64*1024;

      cardBlocks_ = (csd.v2.c_size+1)*1024;
    } else {
      assert(csd.v1.csd_structure == 0);
    
      blockSize_ = _pow2(csd.v1.read_bl_len);
    
      sectorSize_ = blockSize_*(csd.v1.sector_size+1);
    
      cardBlocks_ = (csd.v1.c_size+1)*_pow2(csd.v1.c_size_mult+2);
    }

    if(!sd) {
      changeClock(5); // 19.2 MHz
      writeTimeout_ = 11; // ~500 ms
      readTimeout_ = 8; // ~100 ms
    } else {
      // TODO: Check card for High-Speed (50 MHz) support
      changeClock(4); // 24 MHz
      writeTimeout_ = 11; // ~ 500 ms
      readTimeout_ = 9; // ~ 100 ms
    }

    if(sd) {
      if(sdv2) {
	if(csd.v1.csd_structure == 1)
	  if(cardBlocks_ >= 67108864u) // 32GiB
	    cardType_ = CardType::sdxc;
	  else
	    cardType_ = CardType::sdhc;
	else
	  cardType_ = CardType::sdv2;
      } else
	cardType_ = CardType::sdv1;
    } else
      cardType_ = CardType::mmc;

    printf("Detected %s card, %u KiB (%u byte Blocks, %u byte Sectors)\n",
	   cardTypeToString(), cardBlocks_*blockSize_/1024, blockSize_, sectorSize_);

    resp = command_r1b(7, rca_<<16);
    printf("%.8lx\n", resp);

    if(cardType_ != CardType::mmc) {
      resp = command_r1(ACMD(6), 0x2); // Set 4 byte bus on card
      
      r_hctl() |= (1<<1); // Set 4 byte bus on controller
    }

    resp = command_r1(16, 512);
    printf("%.8lx\n", resp);

    try {
      test_data(0);
    } catch(dataerror &ex) {
      printf("Data error: %.8lx\n", ex.stat);
    }
  } catch(commanderror &ex) {
    printf("Unexpected command error: %.8lx\n", ex.stat);
    throw;
  }
}

OMAP35x_SDMMC::~OMAP35x_SDMMC() {
  global::intc->disable_int(irq_);
}

unsigned OMAP35x_SDMMC::getBlockSize() const {
  return blockSize_;
}

void OMAP35x_SDMMC::write(unsigned pos, char const* data, unsigned count) {
  throw ex::error{ENOTSUP};
}
  
void OMAP35x_SDMMC::read(unsigned pos, char *data, unsigned count) {
  throw ex::error{ENOTSUP};
}

void OMAP35x_SDMMC::command(unsigned cmd, uint32_t arg) {
  assert(intSem_.getCount() == 0);

  if(cmd&0x100) {
    acmd();
    cmd &= 0xff;
  }
  assert(cmd <= 63);

  while(r_pstate() & (1<<0)); // Wait for command line free

  r_con() &= ~(1<<6) & ~(1<<3);
  r_csre() = 0x0;
  
  r_blk() &= 0x0000f800; // No data transfer

  r_stat() = 0xffffffff;

  r_arg() = arg;

  uint32_t creg = (cmd<<24);
  if(cmd == 12)
    creg |= (0x3<<22);
  creg |= // (1<<19) |
    (0<<16);
  
  r_cmd() = creg;

  intSem_.acquire();
  
  if(intStat_ & (1<<16)) {
    cmd_reset();
    throw ex::timeout{};
  } else if(intStat_ & (1<<15)) {
    throw commanderror{intStat_};
  }

  assert(intStat_ & (1<<0));
}

uint32_t OMAP35x_SDMMC::command_r1(unsigned cmd, uint32_t arg) {
  assert(intSem_.getCount() == 0);

  if(cmd&0x100) {
    acmd();
    cmd &= 0xff;
  }
  assert(cmd <= 63);

  while(r_pstate() & (1<<0)); // Wait for command line free

  r_con() &= ~(1<<6) & ~(1<<3);
  r_csre() = 0x0;
  
  r_blk() &= 0x0000f800; // No data transfer

  r_stat() = 0xffffffff;

  r_arg() = arg;

  uint32_t creg = (cmd<<24);
  if(cmd == 12)
    creg |= (0x3<<22);
  if(!(cmd == 41)) // These commands do not respond with CRC, otherwise
    creg |= (1<<19); // CRC check enable
  if(!(cmd == 41)) // These commands do not respond with command index, otherwise
    creg |= (1<<20); // Index check enable
  creg |= (2<<16);

  r_cmd() = creg;

  intSem_.acquire();

  if(intStat_ & (1<<16)) {
    cmd_reset();
    throw ex::timeout{};
  } else if(intStat_ & (1<<15)) {
    throw commanderror{intStat_};
  } 

  assert(intStat_ & (1<<0));

  return r_rsp10();
}

uint32_t OMAP35x_SDMMC::command_r1b(unsigned cmd, uint32_t arg) {
  assert(intSem_.getCount() == 0);

  if(cmd&0x100) {
    acmd();
    cmd &= 0xff;
  }
  assert(cmd <= 63);

  while(r_pstate() & (1<<0)); // Wait for command line free

  r_sysctl() = (r_sysctl() & ~(0xf<<16)) | (readTimeout_<<16); // Use read timeout

  r_con() &= ~(1<<6) & ~(1<<3);
  r_csre() = 0x0;
  
  r_blk() &= 0x0000f800; // No data transfer

  r_stat() = 0xffffffff;

  r_arg() = arg;

  uint32_t creg = (cmd<<24);
  if(cmd == 12)
    creg |= (0x3<<22);
  creg |= (1<<19) | (1<<20); // CRC check, index check enable
  creg |= // (1<<19) | 
    (3<<16);

  r_cmd() = creg;

  intSem_.acquire();

  if(intStat_ & (1<<16)) {
    cmd_reset();
    throw ex::timeout{};
  } else if(intStat_ & (1<<15)) {
    throw commanderror{intStat_};
  } 

  assert(intStat_ & (1<<0));

  return r_rsp10();
}

std::array<uint32_t, 4> OMAP35x_SDMMC::command_r2(unsigned cmd, uint32_t arg) {
  assert(intSem_.getCount() == 0);

  if(cmd&0x100) {
    acmd();
    cmd &= 0xff;
  }
  assert(cmd <= 63);

  while(r_pstate() & (1<<0)); // Wait for command line free

  r_con() &= ~(1<<6) & ~(1<<3);
  r_csre() = 0x0;
  
  r_blk() &= 0x0000f800; // No data transfer

  r_stat() = 0xffffffff;

  r_arg() = arg;

  uint32_t creg = (cmd<<24);
  if(cmd == 12)
    creg |= (0x3<<22);
  creg |= (1<<19); // CRC check enable
  creg |= (1<<16);

  r_cmd() = creg;

  intSem_.acquire();

  if(intStat_ & (1<<16)) {
    cmd_reset();
    throw ex::timeout{};
  } else if(intStat_ & (1<<15)) {
    throw commanderror{intStat_};
  } 

  assert(intStat_ & (1<<0));

  return std::array<uint32_t, 4>{r_rsp10(), r_rsp32(), r_rsp54(), r_rsp76()};
}

void OMAP35x_SDMMC::int_handler() {
  intStat_ = r_stat();
  r_stat() = intStat_; // Read and clear status bits
  
  intSem_.release();

  global::prcm->clear_wake_core(24);
}

void OMAP35x_SDMMC::acmd() {
  uint32_t resp = command_r1(55, rca_<<16);
  if(!(resp & (1<<5))) { // Not in ACMD mode
    // WTF?
    printf("Error: CMD55 response without APP_CMD set (%.8lx)\n", resp);
    throw ex::error{EIO};
  }
}

void OMAP35x_SDMMC::cmd_reset() {
  r_sysctl() |= (1<<25);
  while(r_sysctl() & (1<<25));
}

void OMAP35x_SDMMC::dat_reset() {
  r_sysctl() |= (1<<26);
  while(r_sysctl() & (1<<26));
}

void OMAP35x_SDMMC::changeClock(unsigned cdiv) {
  assert(cdiv <= 0x3ff);

  r_sysctl() &= ~(1<<2) & ~(1<<0); // Clock disable

  r_sysctl() &= ~(0x3ff<<6); // clear clock divider
  r_sysctl() |= (cdiv<<6) | (1<<0); // Set new divider and start internal clock
  while(!(r_sysctl() & (1<<1))); // Wait until clock stable

  r_sysctl() |= (1<<2); // Clock reenable
}

char const* OMAP35x_SDMMC::cardTypeToString() const {
  switch(cardType_) {
  case CardType::sdxc:
    return "SDxc";
  case CardType::sdhc:
    return "SDhc";
  case CardType::sdv2:
    return "SDsc v2";
  case CardType::sdv1:
    return "SDsc v1";
  case CardType::mmc:
    return "MMC";
  default:
    return "Unknown";
  }
}

void OMAP35x_SDMMC::test_data(unsigned block) {
  assert(intSem_.getCount() == 0);

  std::vector<uint32_t> buffer(128, 0);

  while(r_pstate() & (1<<0)); // Wait for command line free
  while(r_pstate() & (1<<1)); // Wait for data line free

  r_sysctl() = (r_sysctl() & ~(0xf<<16)) | (readTimeout_<<16); // Use read timeout

  r_con() &= ~(1<<6) & ~(1<<3);
  r_csre() = 0x0;
  
  r_blk() &= 0x0000f800;
  r_blk() |= 0x00010000 | blockSize_;

  r_stat() = 0xffffffff;

  if((cardType_ == CardType::sdxc) || (cardType_ == CardType::sdhc))
    r_arg() = block;
  else
    r_arg() = block*blockSize_;

  uint32_t creg = (17<<24); // CMD17 read single block
  creg |= (1<<21) | (1<<20) | (1<<19) | (2<<16) | (1<<4);

  r_cmd() = creg;

  intSem_.acquire();

  if(intStat_ & (1<<16)) {
    cmd_reset();
    throw ex::timeout{};
  } else if(intStat_ & (1<<15)) {
    throw commanderror{intStat_};
  } 

  assert(intStat_ & (1<<0));
 
  // TODO: DMA for data transfers
  unsigned pos = 0;
  while(true) {
    uint32_t stat = r_stat() | intStat_; // Make sure no status bits get 'lost'
    intStat_ = 0;
    if(stat & (1<<15)) {
      dat_reset();
      throw dataerror{stat};
    } else if(stat & (1<<1)) {
      break;
    } else if(stat & (1<<5)) {
      assert(pos < 128);
      buffer[pos++] = r_data();
    }
  }
  
  for(int i = 0;i < 32;++i) {
    for(int j = 0;j < 4;++j) {
      printf("%.2lx %.2lx %.2lx %.2lx ",
	     buffer[i*4+j]&0xff,
	     (buffer[i*4+j]>>8)&0xff,
	     (buffer[i*4+j]>>16)&0xff,
	     (buffer[i*4+j]>>24)&0xff);
    }
    printf("\n");
  }
  
}