#include <stdio.h>
#include <stm32h7xx.h>
#include "W25Qxx.h"

QSPI_HandleTypeDef hqspi;

void W25Qxx_EnableWrite(void)
{
  QSPI_CommandTypeDef cmd;
 
  cmd.AddressMode = QSPI_ADDRESS_NONE;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_NONE;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 0;
  cmd.NbData = 0;
  cmd.Instruction = 0x06;
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
}

void W25Qxx_EraseSector(uint16_t sector)
{
  QSPI_CommandTypeDef cmd;
 
  W25Qxx_EnableWrite();
  cmd.Address = sector << 12;
  cmd.AddressMode = QSPI_ADDRESS_1_LINE;
  cmd.AddressSize = QSPI_ADDRESS_24_BITS;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_NONE;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 0;
  cmd.NbData = 0;
  cmd.Instruction = 0x20;
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
 
  while (W25Qxx_ReadStatus(1) & W25Qxx_STATUS_BUSY);
}

void W25Qxx_Init(void)
{
  uint8_t m, status;
  uint16_t id;
  GPIO_InitTypeDef gpio;

  __HAL_RCC_GPIOF_CLK_ENABLE();
  __HAL_RCC_GPIOG_CLK_ENABLE();
  __HAL_RCC_QSPI_CLK_ENABLE();
 
  gpio.Alternate = GPIO_AF10_QUADSPI;
  gpio.Mode = GPIO_MODE_AF_PP;
  gpio.Pin = GPIO_PIN_8 | GPIO_PIN_9;
  gpio.Pull = GPIO_NOPULL;
  gpio.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
  HAL_GPIO_Init(GPIOF, &gpio);
 
  gpio.Alternate = GPIO_AF9_QUADSPI;
  gpio.Pin = GPIO_PIN_6 | GPIO_PIN_7 | GPIO_PIN_10;
  HAL_GPIO_Init(GPIOF, &gpio);
 
  gpio.Alternate = GPIO_AF10_QUADSPI;
  gpio.Pin = GPIO_PIN_6;
  HAL_GPIO_Init(GPIOG, &gpio);
 
  hqspi.Instance = QUADSPI;
  hqspi.Init.ChipSelectHighTime = QSPI_CS_HIGH_TIME_1_CYCLE;
  hqspi.Init.ClockMode = QSPI_CLOCK_MODE_0;
  hqspi.Init.ClockPrescaler = 1;
  hqspi.Init.DualFlash = QSPI_DUALFLASH_DISABLE;
  hqspi.Init.FifoThreshold = 32;
  hqspi.Init.FlashID = QSPI_FLASH_ID_1;
  hqspi.Init.FlashSize = 23; // W25Q128 (2^(23+1)=16MB)
  hqspi.Init.SampleShifting = QSPI_SAMPLE_SHIFTING_HALFCYCLE;
  HAL_QSPI_Init(&hqspi);
 
  id = W25Qxx_ReadID(&m);
  printf("SPI Flash: M=0x%02x, ID=0x%02x\n", m, id);
  id = W25Qxx_ReadJEDECID(&m);
  printf("SPI Flash: M=0x%02x, ID=0x%04x\n", m, id);
 
  status = W25Qxx_ReadStatus(2);
  if ((status & W25Qxx_STATUS2_QE) == 0)
  {
    printf("QUADSPI mode is disabled\n");
    while (1);
  }
}

void W25Qxx_ProgramPage(uint32_t addr, const void *data, int len)
{
  QSPI_CommandTypeDef cmd;
 
  W25Qxx_EnableWrite();
  cmd.Address = addr;
  cmd.AddressMode = QSPI_ADDRESS_1_LINE;
  cmd.AddressSize = QSPI_ADDRESS_24_BITS;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_4_LINES;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 0;
  cmd.NbData = len;
  cmd.Instruction = 0x32;
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
  HAL_QSPI_Transmit(&hqspi, (uint8_t *)data, HAL_MAX_DELAY);
 
  while (W25Qxx_ReadStatus(1) & W25Qxx_STATUS_BUSY);
}

void W25Qxx_Read(uint32_t addr, void *data, int len)
{
  QSPI_CommandTypeDef cmd;
 
  W25Qxx_EnableWrite();
  cmd.Address = addr;
  cmd.AddressMode = QSPI_ADDRESS_4_LINES;
  cmd.AddressSize = QSPI_ADDRESS_24_BITS;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_4_LINES;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 6;
  cmd.NbData = len;
  cmd.Instruction = 0xeb;
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
  HAL_QSPI_Receive(&hqspi, data, HAL_MAX_DELAY);
}

uint8_t W25Qxx_ReadID(uint8_t *pm)
{
  uint8_t data[5];
  uint8_t m, id;
  QSPI_CommandTypeDef cmd;
 
  cmd.AddressMode = QSPI_ADDRESS_NONE;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_1_LINE;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 0;
  cmd.NbData = sizeof(data);
  cmd.Instruction = 0x90;
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
  HAL_QSPI_Receive(&hqspi, data, HAL_MAX_DELAY);
 
  m = data[3];
  if (pm != NULL)
    *pm = m;
  id = data[4];
  return id;
}

uint16_t W25Qxx_ReadJEDECID(uint8_t *pm)
{
  uint8_t data[3];
  uint8_t m;
  uint16_t id;
  QSPI_CommandTypeDef cmd;
 
  cmd.AddressMode = QSPI_ADDRESS_NONE;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_1_LINE;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 0;
  cmd.NbData = sizeof(data);
  cmd.Instruction = 0x9f;
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
  HAL_QSPI_Receive(&hqspi, data, HAL_MAX_DELAY);
 
  m = data[0];
  if (pm != NULL)
    *pm = m;
  id = (data[1] << 8) | data[2];
  return id;
}

uint8_t W25Qxx_ReadStatus(int i)
{
  const uint8_t code[] = {0x05, 0x35, 0x15};
  uint8_t status;
  QSPI_CommandTypeDef cmd;
 
  assert_param(i >= 1 && i <= sizeof(code));
  cmd.AddressMode = QSPI_ADDRESS_NONE;
  cmd.AlternateByteMode = QSPI_ALTERNATE_BYTES_NONE;
  cmd.DataMode = QSPI_DATA_1_LINE;
  cmd.DdrMode = QSPI_DDR_MODE_DISABLE;
  cmd.DdrHoldHalfCycle = QSPI_DDR_HHC_ANALOG_DELAY;
  cmd.DummyCycles = 0;
  cmd.NbData = 1;
  cmd.Instruction = code[i - 1];
  cmd.InstructionMode = QSPI_INSTRUCTION_1_LINE;
  cmd.SIOOMode = QSPI_SIOO_INST_EVERY_CMD;
  HAL_QSPI_Command(&hqspi, &cmd, HAL_MAX_DELAY);
  HAL_QSPI_Receive(&hqspi, &status, HAL_MAX_DELAY);
  return status;
}

#ifndef __W25Qxx_H
#define __W25Qxx_H

#define W25Qxx_STATUS_BUSY 0x01
#define W25Qxx_STATUS_WEL 0x02 // Write Enable Latch
#define W25Qxx_STATUS2_QE 0x02 // Quad Enable

void W25Qxx_EnableWrite(void);
void W25Qxx_EraseSector(uint16_t sector);
void W25Qxx_Init(void);
void W25Qxx_ProgramPage(uint32_t addr, const void *data, int len);
void W25Qxx_Read(uint32_t addr, void *data, int len);
uint8_t W25Qxx_ReadID(uint8_t *pm);
uint16_t W25Qxx_ReadJEDECID(uint8_t *pm);
uint8_t W25Qxx_ReadStatus(int i);

#endif