#include <memory>
#include <cstdint>
#include <cstdio>
#include <cassert>

#include "util.hh"
#include "mmio.hh"
#include "exceptions.hh"
#include "drv_omap35x_i2c.hh"

namespace ex {
  class i2c_tmp_fail {};
  class i2c_nack : public i2c_tmp_fail {};
  class i2c_arbitration_lost : public i2c_tmp_fail {};
  
  class i2c_timeout : public timeout {};
  class i2c_error : public error {};
}

class OMAP35x_I2C_impl {
public:
  OMAP35x_I2C_impl(uintptr_t base) : base_{base}, xtrsh_{1}, rtrsh_{1} {
    r_sysc() = 0x011;
    r_psc() = 0x7; // Set clock prescaler to /8 (=12MHz internal clock)
    r_scll() = 0x9; // Set low pulse width for 400kHz mode (t_low = 1.3_us)
    r_sclh() = 0x9; // Set high pulse width for 400kHz mode (t_high = 1.16_us)
    r_oa0() = 0x20; // Set own address to 0x20
    r_con() = 0x00008400; // Enable module in master mode
  }

  ~OMAP35x_I2C_impl() {
  }

  void reg_write(uint8_t slaveid, uint8_t reg, uint8_t value) {
    transmit(slaveid, {reg, value});
  }

  uint8_t reg_read(uint8_t slaveid, uint8_t reg) {
    auto tmp = transfer2(slaveid, {reg}, 1);
    return tmp[0];
  }

  void reg_write16(uint8_t slaveid, uint8_t reg, uint16_t value) {
    transmit(slaveid, {reg, value&0xff, value>>8});
  }

  uint16_t reg_read16(uint8_t slaveid, uint8_t reg) {
    auto tmp = transfer2(slaveid, {reg}, 2);
    return tmp[0] | (tmp[1]<<8);
  }

private:
  // Send the bytes in 'data' to I2C slave 'slaveid'
  void transmit(uint8_t slaveid, std::string const& data) {
    bool done = false;
    while(!done) {
      _wait_busfree();

      r_sa() = slaveid&0x7f; // Set slave address
      r_cnt() = data.size(); // Set transfer length
      r_con() = 0x8603; // master transmit, generate start and stop cond.
     
      try {
	_send(data);
	done = true;
      } catch(ex::i2c_tmp_fail& ex) {
	printf("Error, restart transfer\n");
      }
    }
  }

  // Two-phase transfer:
  // Send the bytes in 'data' to I2C slave 'slaveid',
  // then receive 'recvcount' bytes
  std::string transfer2(uint8_t slaveid, std::string const& data, size_t recvcount) {
    while(true) {
      _wait_busfree();
      
      r_sa() = slaveid&0x7f; // Set slave address
      r_cnt() = data.size(); // Set transfer length
      r_con() = 0x8601; // master transmit, generate start cond.

      try {
	_send(data);
      } catch(ex::i2c_tmp_fail& ex) {
	continue; // restart transfer
      }
      
      r_cnt() = recvcount;
      r_con() = 0x8403; // master receive, generate start and stop cond.

      std::string res;
      try {
	_recv(res);
      } catch(ex::i2c_arbitration_lost& ex) {
	// Should not happen
	throw ex::i2c_error{};
      } catch(ex::i2c_tmp_fail& ex) {
	continue; // restart transfer
      }
      return res;
    }
  }

  void _send(std::string const& data) {
    size_t pos = 0;
    while(true) {
      auto stat = r_stat();
      if(stat & (1<<1)) { // NACK
	r_stat() = (1<<1); // clear NACK bit
        throw ex::i2c_nack{};
      } else if (stat & (1<<0)) { // ARB lost
	r_stat() = (1<<0); // clear AL bit
	throw ex::i2c_arbitration_lost{};
      } else if (stat & (1<<2)) { // ARDY (xfer complete)
	r_stat() = (1<<2); // clear ARDY bit
	return;
      } else if (stat & (1<<14)) { // XDR
	unsigned writecnt = r_bufstat() & 0x3f;
	assert(writecnt <= data.size()-pos);
	for(size_t i = 0;i < writecnt;++i)
	  r_data() = data[i+pos]&0xff;
	pos += writecnt;
	r_stat() = (1<<14); // clear XDR bit
      } else if (stat & (1<<4)) { // XRDY
	assert(xtrsh_ <= data.size()-pos);
	for(size_t i = 0;i < xtrsh_;++i)
	  r_data() = data[i+pos]&0xff;
	pos += xtrsh_;
	r_stat() = (1<<4); // clear XRDY bit
      }
    }
  }

  void _recv(std::string& data) {
    while(true) {
      auto stat = r_stat();
      if(stat & (1<<1)) { // NACK
	r_stat() = (1<<1); // clear NACK bit
        throw ex::i2c_nack{};
      } else if (stat & (1<<0)) { // ARB lost
	r_stat() = (1<<0); // clear AL bit
	throw ex::i2c_arbitration_lost{};
      } else if (stat & (1<<2)) { // ARDY (xfer complete)
	r_stat() = (1<<2); // clear ARDY bit
	return;
      } else if (stat & (1<<13)) { // RDR
	unsigned readcnt = (r_bufstat() & 0x3f00)>>8;
	for(size_t i = 0;i < readcnt;++i)
	  data.push_back(r_data()&0xff);
	r_stat() = (1<<13); // clear RDR bit
      } else if (stat & (1<<3)) { // RRDY
	for(size_t i = 0;i < rtrsh_;++i)
	  data.push_back(r_data()&0xff);
	r_stat() = (1<<3); // clear RRDY bit
      }
    }
  }

  void _wait_busfree() {
    // Wait while bus busy = 1
    while(r_stat() & (1<<12)) {}
  }

  MMIO_alloc base_;
  uint8_t xtrsh_, rtrsh_;

  uint8_t volatile& r_rev() { return _reg8(base_.get_virt(), 0x0); }
  uint16_t volatile& r_ie() { return _reg16(base_.get_virt(), 0x4); }
  uint16_t volatile& r_stat() { return _reg16(base_.get_virt(), 0x8); }
  uint16_t volatile& r_we() { return _reg16(base_.get_virt(), 0xc); }
  uint8_t volatile& r_syss() { return _reg8(base_.get_virt(), 0x10); }
  uint16_t volatile& r_buf() { return _reg16(base_.get_virt(), 0x14); }
  uint16_t volatile& r_cnt() { return _reg16(base_.get_virt(), 0x18); }
  uint8_t volatile& r_data() { return _reg8(base_.get_virt(), 0x1c); }
  uint16_t volatile& r_sysc() { return _reg16(base_.get_virt(), 0x20); }
  uint16_t volatile& r_con() { return _reg16(base_.get_virt(), 0x24); }
  uint16_t volatile& r_oa0() { return _reg16(base_.get_virt(), 0x28); }
  uint16_t volatile& r_sa() { return _reg16(base_.get_virt(), 0x2c); }
  uint16_t volatile& r_psc() { return _reg16(base_.get_virt(), 0x30); }
  uint16_t volatile& r_scll() { return _reg16(base_.get_virt(), 0x34); }
  uint16_t volatile& r_sclh() { return _reg16(base_.get_virt(), 0x38); }
  uint16_t volatile& r_systest() { return _reg16(base_.get_virt(), 0x3c); }
  uint16_t volatile& r_bufstat() { return _reg16(base_.get_virt(), 0x40); }
  uint16_t volatile& r_oa1() { return _reg16(base_.get_virt(), 0x44); }
  uint16_t volatile& r_oa2() { return _reg16(base_.get_virt(), 0x48); }
  uint16_t volatile& r_oa3() { return _reg16(base_.get_virt(), 0x4c); }
  uint8_t volatile& r_actoa() { return _reg8(base_.get_virt(), 0x50); }
  uint8_t volatile& r_sblock() { return _reg8(base_.get_virt(), 0x54); }
};


OMAP35x_I2C::OMAP35x_I2C(uintptr_t base) : impl_{new OMAP35x_I2C_impl{base}} {
}

OMAP35x_I2C::~OMAP35x_I2C() {
}

void OMAP35x_I2C::reg_write(uint8_t slaveid, uint8_t reg, uint8_t value) {
  impl_->reg_write(slaveid, reg, value);
}

uint8_t OMAP35x_I2C::reg_read(uint8_t slaveid, uint8_t reg) {
  return impl_->reg_read(slaveid, reg);
}

void OMAP35x_I2C::reg_write16(uint8_t slaveid, uint8_t reg, uint16_t value) {
  impl_->reg_write16(slaveid, reg, value);
}

uint16_t OMAP35x_I2C::reg_read16(uint8_t slaveid, uint8_t reg) {
  return impl_->reg_read16(slaveid, reg);
}