#include "string.h"
#include "norflash.h"
extern "C" {
  extern void delay_init ();
  extern void Delay_Ms       (const unsigned dly);
};
union FlashCommandHeader {
  struct {
    FLASH_COMMANDS cmd : 8;
    uint32_t       adr : 24;       // adresa je BIG ENDIEN - vyšší byte vystupuje po SPI dříve (MSB FIRST).
  }__attribute__((packed));
  uint8_t bytes [4];
}__attribute__((packed));
static inline uint32_t be_set_24 (const uint32_t p) {
  return  ((p & 0x000000ff) << 16)
        | ((p & 0x0000ff00) <<  0)
        | ((p & 0x00ff0000) >> 16);
}
static constexpr unsigned FlashBlockLen = 0x1000u;  // 4KiB = 4096 B

NorFlash::NorFlash() noexcept : spi() {
  delay_init();
}
unsigned int NorFlash::ReadBlock(const unsigned int addr, uint8_t * data, const unsigned int len) {
  FlashCommandHeader header;
  header.cmd = FLASH_4READ;
  header.adr = be_set_24(addr);
  spi.ChipSelect(true);
  for (unsigned n=0u; n<sizeof(header); n++) spi.ReadWriteByte(header.bytes[n]);
  for (unsigned n=0u; n<len; n++) data [n] = spi.ReadWriteByte(0xff);
  spi.ChipSelect(false);
  return len;
}
unsigned int NorFlash::WriteBlock(const unsigned int addr, const uint8_t * data, const unsigned int len) {
  FlashCommandHeader header;
  header.cmd = FLASH_4PP;
  header.adr = be_set_24(addr);
  SingleCommand(FLASH_WREN);
  spi.ChipSelect(true);
  for (unsigned n=0u; n<sizeof(header); n++) spi.ReadWriteByte(header.bytes[n]);
  for (unsigned n=0u; n<len; n++)            spi.ReadWriteByte(data [n]);
  spi.ChipSelect(false);
  if (!WaitForReady()) return 0;  
  return len;
}
bool NorFlash::EraseSector(const unsigned int addr) {
  FlashCommandHeader header;
  header.cmd = FLASH_4SE;
  header.adr = be_set_24(addr);
  SingleCommand(FLASH_WREN);
  spi.ChipSelect(true);
  for (unsigned n=0u; n<sizeof(header); n++) spi.ReadWriteByte(header.bytes[n]);
  spi.ChipSelect(false);
  if (!WaitForReady(400u)) return false;  
  return true;
}
//////////////////////////////////////////////////////////////////////////////////////////////////////////
inline bool NorFlash::SingleCommand(const FLASH_COMMANDS cmd) const {
  uint8_t tmp = cmd;
  spi.ChipSelect(true);
  spi.ReadWriteByte (tmp);
  spi.ChipSelect(false);
  return true;
}
inline uint8_t NorFlash::ReadStatus(const FLASH_COMMANDS cmd) const {
  uint8_t buf [2] = {cmd, 0};
  spi.ChipSelect(true);
  for (unsigned n=0u; n<2u; n++) buf[n] = spi.ReadWriteByte(buf[n]);
  spi.ChipSelect(false);
  return buf[1];
}
bool NorFlash::WaitForReady(const unsigned int ms) const {
  uint8_t sr;
  for (unsigned n=0; n<ms; n++) {
    Delay_Ms (1u);
    sr = ReadStatus(FLASH_RDSR1);
    if ((sr & 1u) == 0u) return true;
  }
  return false;
}