// NOTE: most of the code here is translated from a Rust crate
// https://github.com/stm32-rs/stm32-fmc

#include "fmc.h"
#include "registers.h"
#include "stm32h747xx.h"
#include "delay.h"

void fmc_init(fmc_t *fmc) {
  fmc->bank1 = FMC_Bank1_R;
  fmc->bank2 = FMC_Bank2_R;
  fmc->bank3 = FMC_Bank3_R;
  fmc->bank5_6 = FMC_Bank5_6_R;

  fmc->sdram[0].allocated = 0;
  fmc->sdram[0].base = (uint32_t*)0xC0000000;
  fmc->sdram[1].allocated = 0;
  fmc->sdram[1].base = (uint32_t*)0xD0000000;
}

uint32_t max(uint32_t a, uint32_t b) {
  return (a > b) ? a : b;
}

void fmc_sdram_configure(fmc_t *fmc, sdram_bank_t target_bank, fmc_sdram_configuration_t config,
                         fmc_sdram_timing_t timing, uint16_t mode_register) {
  reg_write_bits(&fmc->bank5_6->SDCR[0],
                 (config.read_pipe_delay_cycles << FMC_SDCRx_RPIPE_Pos) |
                 (config.read_burst << FMC_SDCRx_RBURST_Pos) |
                 (2 << FMC_SDCRx_SDCLK_Pos),
                 FMC_SDCRx_RPIPE_Msk | FMC_SDCRx_RBURST_Msk | FMC_SDCRx_SDCLK_Msk);

  uint8_t internal_banks;
  switch (config.internal_banks) {
  case 2:
    internal_banks = 0;
    break;
  case 4:
    internal_banks = 1;
    break;
  default:
    // Error!
    break;
  }

  uint8_t memory_data_width;
  switch (config.memory_data_width) {
  case 8:
    memory_data_width = 0;
    break;
  case 16:
    memory_data_width = 1;
    break;
  case 32:
    memory_data_width = 2;
    break;
  default:
    // Error!
    break;
  }

  reg_write_bits(&fmc->bank5_6->SDCR[target_bank],
                 (config.write_protection << FMC_SDCRx_WP_Pos) |
                     (config.cas_latency << FMC_SDCRx_CAS_Pos) |
                     (internal_banks << FMC_SDCRx_NB_Pos) |
                     (memory_data_width << FMC_SDCRx_MWID_Pos) |
                     ((config.row_bits - 11) << FMC_SDCRx_NR_Pos) |
                     ((config.column_bits - 8) << FMC_SDCRx_NC_Pos),
                 FMC_SDCRx_WP_Msk | FMC_SDCRx_CAS_Msk |
                 FMC_SDCRx_NB_Msk | FMC_SDCRx_MWID_Msk |
                 FMC_SDCRx_NR_Msk | FMC_SDCRx_NC_Msk);

  // Timing ---- SDTR REGISTER

  // Self refresh >= ACTIVE to PRECHARGE
  uint32_t minimum_self_refresh = timing.active_to_precharge;

  // Write recovery - Self refresh
  uint32_t write_recovery_self_refresh =
      minimum_self_refresh - timing.row_to_column;
  // Write recovery - WRITE command to PRECHARGE command
  uint32_t write_recovery_row_cycle =
      timing.row_cycle - timing.row_to_column - timing.row_precharge;
  uint32_t write_recovery =
      max(write_recovery_self_refresh, write_recovery_row_cycle);

  reg_write_bits(&fmc->bank5_6->SDTR[0],
                 ((timing.row_cycle - 1) << FMC_SDTRx_TRC_Pos) |
                 ((timing.row_precharge - 1) << FMC_SDTRx_TRP_Pos),
                 FMC_SDTRx_TRC_Msk | FMC_SDTRx_TRP_Msk);

  reg_write_bits(&fmc->bank5_6->SDTR[target_bank],
                 ((timing.row_to_column - 1)) << FMC_SDTRx_TRCD_Pos |
                 ((write_recovery - 1)) << FMC_SDTRx_TWR_Pos |
                 ((minimum_self_refresh - 1)) << FMC_SDTRx_TRAS_Pos |
                 ((timing.exit_self_refresh - 1)) << FMC_SDTRx_TXSR_Pos |
                 ((timing.mode_register_to_active - 1)) << FMC_SDTRx_TMRD_Pos,
                 FMC_SDTRx_TRCD_Msk | FMC_SDTRx_TWR_Msk |
                 FMC_SDTRx_TRAS_Msk | FMC_SDTRx_TXSR_Msk |
                 FMC_SDTRx_TMRD_Msk);


  // TODO: is this right?
  reg_set_bits(&fmc->bank1->BTCR[0], FMC_BCR1_FMCEN_Msk);

  fmc_sdram_send_command(fmc, target_bank, SDRAM_CLK_ENABLE, 0);

  delay_us(timing.startup_delay_us);

  fmc_sdram_send_command(fmc, target_bank, SDRAM_PRECHARGE_ALL, 0);
  fmc_sdram_send_command(fmc, target_bank, SDRAM_AUTOREFRESH, 8);
  fmc_sdram_send_command(fmc, target_bank, SDRAM_LOAD_MODE, mode_register);

  uint64_t refresh_counter_top = (((uint64_t)timing.refresh_period_ns
                                   * (uint64_t)timing.max_sd_clock_hz)
                                  / 1000000000) - 20;

  reg_write_bits(&fmc->bank5_6->SDRTR,
                 (refresh_counter_top << FMC_SDRTR_COUNT_Pos), FMC_SDRTR_COUNT_Msk);
}

void *fmc_sdram_allocate(fmc_t *fmc, sdram_bank_t bank, uint32_t size) {
  size = ((size + 3) / 4) * 4;

  uint32_t *data = (fmc->sdram[bank].base + fmc->sdram[bank].allocated);
  fmc->sdram[bank].allocated += size;

  return (void*)data;
}

void fmc_sdram_send_command(fmc_t *fmc, uint16_t bank, fmc_sdram_cmd_type_t cmd,
                            uint16_t argument) {

  uint32_t cmd_type, number_refresh, mode_reg;

  switch (cmd) {
  case SDRAM_NORMAL_MODE:
    cmd_type = 0;
    number_refresh = 1;
    mode_reg = 0;
    break;
  case SDRAM_CLK_ENABLE:
    cmd_type = 1;
    number_refresh = 1;
    mode_reg = 0;
    break;
  case SDRAM_PRECHARGE_ALL:
    cmd_type = 2;
    number_refresh = 1;
    mode_reg = 0;
    break;
  case SDRAM_AUTOREFRESH:
    cmd_type = 3;
    number_refresh = argument;
    mode_reg = 0;
    break;
  case SDRAM_LOAD_MODE:
    cmd_type = 4;
    number_refresh = 1;
    mode_reg = argument;
    break;
  case SDRAM_SELFREFRESH:
    cmd_type = 5;
    number_refresh = 1;
    mode_reg = 0;
    break;
  case SDRAM_POWERDOWN:
    cmd_type = 6;
    number_refresh = 0;
    mode_reg = 0;
    break;
  }

  uint32_t b1, b2;
  switch (bank) {
  case 0:
    b1 = 1;
    b2 = 0;
    break;
  case 1:
    b1 = 0;
    b2 = 1;
    break;
  }

  fmc->bank5_6->SDCMR =
    (mode_reg << FMC_SDCMR_MRD_Pos) |
    (number_refresh << FMC_SDCMR_NRFS_Pos) |
    (b1 << FMC_SDCMR_CTB1_Pos) |
    (b2 << FMC_SDCMR_CTB2_Pos) |
    (cmd_type << FMC_SDCMR_MODE_Pos);
}