#include <vstypes.h>
#include <vo_stdio.h>
#include <vsrv.h>
#include <clockspeed.h>
#include <scheduler.h>
#include <string.h>
#include <math.h>
#include <apploader.h>
#include <uartspeed.h>
#include "sdram.h"


#if 0
/* In case multitasking isn't working properly. */
#  define AVOID_DELAY
#endif

int sdrIoWidthIdx, sdrDensity, sdrType, sdrBanks, sdr12MHz;
static char *sdrTypeName[4] = {"S4 SDRAM", "S2 SDRAM", "N NVM", "?"};

#define SD_TYPE_S4SDRAM 0
#define SD_TYPE_S2SDRAM 1
#define SD_TYPE_NNVM    2


/* if single32BitWord == 1, write one 32-bit word, otherwise write nx64 bits */
void SdRamWriteCmd(register u_int32 addr, register u_int16 single32BitWord) {
#if 0
  if (addr*2 >= 0xc59000 && addr*2 <= 0xc59400) {
    printf("## SdRamWriteCmd(addr=%lx, single=%d\n", addr*2, single32BitWord);
  }
#endif
  PERIP(SDRAM_CA_HI) = (u_int16)(addr>>(16-SDRAM_CA_LO_ADDRLO_B));
  PERIP(SDRAM_CA_LO) = (u_int16)(addr<<SDRAM_CA_LO_ADDRLO_B) |
    (SDRAM_CA_LO_CMD_WRITE | (single32BitWord?SDRAM_CA_LO_SHORT:0));
}

/* if single32BitWord == 1, read one 32-bit word, otherwise read 8x32 bits */
void SdRamReadCmd(register u_int32 addr, register u_int16 single32BitWord) {
  PERIP(SDRAM_CA_HI) = (u_int16)(addr>>(16-SDRAM_CA_LO_ADDRLO_B));
  PERIP(SDRAM_CA_LO) = (u_int16)(addr<<SDRAM_CA_LO_ADDRLO_B) |
    (SDRAM_CA_LO_CMD_READ | (single32BitWord?SDRAM_CA_LO_SHORT:0));
}




int SdRamWaitWriteBufEmpty(void) {
  u_int32 tc = ReadTimeCount();
  while ((PERIP(SDRAM_ST) & 0x0002) == 0) {
    if (ReadTimeCount()-tc >= SD_WRITE_TIMEOUT_MS) return -1;
  }
  return 0;
}

int SdRamWaitReadBufFilled(void) {
  u_int32 tc = ReadTimeCount();
  /* Wait for first data word to appear. After this is true, a new word
     will appear to the buffer every clock cycle. */
  while (!(PERIP(SDRAM_ST) & SDRAM_ST_RX_32BIT_WORDS_AVAILABLE_MASK)) {
    if (ReadTimeCount()-tc >= SD_WRITE_TIMEOUT_MS) return -1;
  }
  return 0;
}



#if 0
int SdRamWrite64N(register __i0 const u_int16 *d, register __a1 u_int16 n) {
  int j;
  for (j=0; j<n; j++) {
    int i;
    if (SdRamWaitWriteBufEmpty()) return -1;
    for (i=0; i<SDRAM_WRITE_LWORDS; i++) {
      PERIP(SDRAM_DATA_HI) = *d++;
      PERIP(SDRAM_DATA_LO) = *d++;
    }
  }
  return 0;
}
#endif

#if 0
int SdRamRead32N(register __reg_c u_int32 wAddr, register __i0 const u_int16 *d, register __a1 u_int16 n) {
  int i, j;
  for (j=0; j<n; j++) {
    SdRamReadCmd(wAddr, 0);
    if (SdRamWaitReadBufFilled()) return -1;
    for (i=0; i<SDRAM_READ_LWORDS; i++) {
      *d++ = PERIP(SDRAM_DATA_HI);
      *d++ = PERIP(SDRAM_DATA_LO);
    }
    wAddr += 2*SDRAM_READ_LWORDS;
  }
  return 0;
}
#endif


#if 1
u_int16 ReadSdReg(register u_int16 regAddr) {
  u_int16 origClkCf = PERIP(CLK_CF);
  u_int16 origVsrv1ClkCf = PERIP(VSRV1_CLK_CF);
  u_int16 res = 0;

  /* Reading registers only work with clk < 49 MHz, so we do it at base
     frequency of 12.288 MHz. */
  if (sdr12MHz) {
    WaitUart0ToClear(100);
    PERIP(CLK_CF) &= ~CLK_CF_FORCEPLL;
    PERIP(VSRV1_CLK_CF) &= ~CLK_CF_FORCEPLL;
  }

  /* VSRVES01 may require up to 4 register reads to get the correct
     result, so repeat read operation 4 times.
     Just in case some memory returns only 8 bits so it may be possible
     that with some memory we'd have to do 8 reads, so do so just in case.
  */
  for (res=0; res<8; res++) {
    PERIP(SDRAM_CF) = 0;
    PERIP(SDRAM_S_REGADDR) = regAddr;
    PERIP(SDRAM_CF) = SDRAM_CF_READSTATUS|SDRAM_CF_VSMODE|SDRAM_CF_MMUENA;
#ifdef AVOID_DELAY
    while (!(PERIP(SDRAM_ST) & SDRAM_ST_INIT_DONE));
#else
    while (!(PERIP(SDRAM_ST) & SDRAM_ST_INIT_DONE)) Delay(1);
#endif
  }
  // Memories may return garbage in bits 15:8, so return only bits 7:0
  res = PERIP(SDRAM_S_REGREAD) & 0xFF;
  if (sdr12MHz) {
    PERIP(CLK_CF) = origClkCf;
    PERIP(VSRV1_CLK_CF) = origVsrv1ClkCf;
  }
  return res;
}
#else
u_int16 ReadSdReg(register u_int16 regAddr) {
  int i;

  //  for (i=0; i<4; i++) {
    PERIP(SDRAM_CF) = 0;
    DelayMicroSec(2);
    PERIP(SDRAM_S_REGADDR) = regAddr;
    DelayMicroSec(2);
    PERIP(SDRAM_CF) = SDRAM_CF_READSTATUS|SDRAM_CF_VSMODE|SDRAM_CF_MMUENA;
 
    printf(":");
    while (!(PERIP(SDRAM_ST) & SDRAM_ST_INIT_DONE)) {
      printf(".");
      Delay(1);
    }
    printf("!%04x!%04x!%04x!%04x!",
	   PERIP(SDRAM_S_REGREAD),
	   PERIP(SDRAM_S_REGREAD),
	   PERIP(SDRAM_S_REGREAD),
	   PERIP(SDRAM_S_REGREAD));
    //  }

  return PERIP(SDRAM_S_REGREAD) & 0xFF;
}
#endif



u_int16 NanoSecToClk(register u_int16 nanoSec, register u_int16 minClk) {
  //Note: Sdram uses 2x CPU clk internally
  u_int16 t = (u_int16)(ceil(nanoSec*1e-9*2*clockSpeed.cpuClkHz));
  if (t < minClk) t = minClk;
  return t;
}


#if 0
#  define PRECALC_DDR_DELAYS
#endif

int InitSdRam(register u_int16 sdrFlags) {
  int res = -1;
#ifdef PRECALC_DDR_DELAYS
  static u_int16 ddrConfig[5][6] = {
    // USDELAY  INIT     ARF MRD_RCD  RP_RFC     WTR
    { 0x000c, 0x1ac8, 0x002d, 0x0503, 0x0203, 0x0003 }, // 12 MHz
    { 0x0019, 0x1ac8, 0x005f, 0x0503, 0x0403, 0x0003 }, // 24 MHz
    { 0x0033, 0x1ac8, 0x00be, 0x0503, 0x0703, 0x0003 }, // 50 MHz
    { 0x0066, 0x1ac8, 0x017c, 0x0503, 0x0e03, 0x0003 }, // 98 MHz, educated guess
  };
  s_int16 configIdx = -1;
#endif

  sdr12MHz = sdrFlags & SDRF_USE_12_MHZ_CLOCK_FOR_INIT;

  /*
    USDELAY = clocks / microsecond
    INIT = 0x1ac8
    ARF  = REFab = tREFI (round downwards, e.g. 3.9 us, then sub 5 clk)
                   see also table LPDDR SDRAM Addressing
    MRD  = tMRW = 5
    RCD  = tRCD = 3 clk / 24 ns, choose larger
    RP   = choose larger of tRPpb,tRPab (note if mem is 4- or 8-bank)
    RFC  = tRFCab
    WTR  = choose larger of tWR,tWTR

    Regs:
    4 - SDRAM Refr. R.
    5 - Manufacturer
    8 - IO Width / Density / Type
  */

  /*

   */
#if 0
  printf("SD Reg 4 = 0x%02x\n", ReadSdReg(0x4));
  printf("SD Reg 4 = 0x%02x\n", ReadSdReg(0x4));
#endif
  /*
    DDR Manufacturer ID's, register 5
	00 Reserved	01 Samsung	02 Qimonda	03 Elpida
	04 Etron	05 Nanya	06 Hynix	07 Mosel
	08 Winbond	09 ESMT		0a Reserved	0b Spansion
	0c SST		0d ZMOS		0e Intel

					fe Mumonyx	ff Micron

	2025-02-19 (Apr 2021):
	0 Reserved	1 Samsung	2 Infineon	3 Elpida
	4 Etron		5 Nanya		6 SK hynix	7 Mosel
	8 Winbond	9 ESMT		A NVM		B ISSI
	C JSC		D AP Memory	E all others	F Micron
   */

#if 0
  Delay(10); /* Wait for the DDR to settle. */
  {
    static u_int16 rNo[] = {4, 4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8,
			    4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8,
			    4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8,
			    4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8,
			    4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8,
			    4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8,
			    4, 4, 4, 5, 5, 5, 5, 8, 8, 8, 8};
    int i;
    for (i=0; i<sizeof(rNo)/sizeof(rNo[1]); i++) {
      int reg = rNo[i];
      printf("ReadSdReg(%d) = %x\n", reg, ReadSdReg(reg));
    }
  }
#endif

  {
    u_int16 t = ReadSdReg(8);	// rIoWidth 7:6, Density 5:2, type 1:0
#if 0
    printf("8=%x\n", t);
#endif
    sdrIoWidthIdx = t >> 6;
    sdrDensity = (t >> 2) & 0xf;
    sdrType = t&3;
  }
  sdrBanks = (sdrDensity < (sdrType ? 6 : 4)) ? 4 : 8;
  if (sdrFlags & SDRF_VERBOSE) {
    const char *manuf[16] = {
     "Reserved", "Samsung", "Infineon", "Elpida",
     "Etron", "Nanya", "SK Hynix", "Mosel",
     "Winbond", "ESMT", "NVM", "ISSI",
     "JSC", "AP Memory", "other", "Micron"
    };
    int reg5 = ReadSdReg(0x5);
#if 0
    printf("r5 %x\n", reg5);
    reg5 = ReadSdReg(0x5);
    printf("r5 %x\n", reg5);
    reg5 = ReadSdReg(0x5);
    printf("r5 %x\n", reg5);
    reg5 = ReadSdReg(0x5);
    printf("r5 %x\n", reg5);
#endif
    printf("DDR parameters:\n");
    printf("  Manufact.: 0x%x (%s)\n", reg5, manuf[reg5 & 0xf]);
    printf("  I/O width: 0x%x -> %d bits\n",
	   sdrIoWidthIdx, sdrIoWidthIdx != 3 ? 32>>sdrIoWidthIdx : 0);
    printf("  Density:   0x%x -> %ld MiB\n", sdrDensity, 8L<<sdrDensity);
    printf("  Type:      0x%x -> %s\n", sdrType, sdrTypeName[sdrType]);
    printf("  Banks:            %d\n", sdrBanks);
  }
  if (sdrIoWidthIdx != 1) {
    printf("E: SD I/O width\n");
    goto finally;
  }
  if (sdrDensity > 9) {
    printf("E: SD density\n");
    goto finally;
  }
  if (sdrType > 1) {
    printf("E: SD type\n");
    goto finally;
  }

  PERIP(SDRAM_CF) = 0x0000; // clear for start

#ifdef PRECALC_DDR_DELAYS
  if (2*clockSpeed.cpuClkHz == 12288000) {
    configIdx = 0;
  } else if (2*clockSpeed.cpuClkHz == 24576000) {
    configIdx = 1;
  } else if (2*clockSpeed.cpuClkHz == 49152000) {
    configIdx = 2;
  } else if (2*clockSpeed.cpuClkHz == 98304000) {
    configIdx = 3;
  }

  if (configIdx < 0) {
    printf("E: Can't find proper DDR configuration for CPU %.3f MHz (Note: Sdram uses internally 2x speed)\n",
	   1e-6*clockSpeed.cpuClkHz);
    res = 1;
    goto finally;
  }

  memcpyXY((void __mem_y *)SDRAM_T_US_DELAY, ddrConfig[configIdx], 6);
#else
  {
    static const u_int16 refreshUs[9] = {156, 156, 78, 78, 78, 39, 39, 39, 39};
    static const u_int16 tRFCab[9] = {90, 90, 90, 90, 130, 130, 130, 210, 210};
    PERIP(SDRAM_T_US_DELAY) = (u_int16)(ceil(1e-6*2*clockSpeed.cpuClkHz));
    PERIP(SDRAM_T_INIT) =0x1ac8;
    /* Note for ARF:
       -6 is to compensate for rounding up (-1) and potential burst (-5) */
    PERIP(SDRAM_T_ARF) = NanoSecToClk(100*refreshUs[sdrDensity], 7)-6;
    PERIP(SDRAM_T_MRD_RCD) = 0x0500|NanoSecToClk(24, 3);
    PERIP(SDRAM_T_RFC_RP) = (NanoSecToClk(tRFCab[sdrDensity], 1)<<8) |
      NanoSecToClk((sdrBanks == 4) ? 24 : 27, 3);
    PERIP(SDRAM_T_WTR) = NanoSecToClk(15, 3);
  }

  {
      //30: DDR_DELAY0
      //dqst0 input  : delay_ctrl 4:0
      //dqst1 input  : delay_ctrl 9:5
      
    u_int16 delayVal1 = 0x9;
    u_int16 delayVal0 = 0x9;
    u_int32 delayReg = 0x0;

    //4:0: dqst0/1
    delayReg |= (u_int32)delayVal0;
    delayReg |= (u_int32)delayVal1 << 5;

    //printf("Delay value to set: 0x%08lx \n", delayReg);
    PERIP(SDRAM_C_DDR_DELAY0) = (u_int16)(delayReg & 0xffff);
    //Debug in ES02 :PERIP(SDRAM_C_DDR_DELAY1) = (u_int16)(delayReg >> 16);
  }
#endif

  PERIP(SDRAM_CF) = SDRAM_CF_VSMODE|SDRAM_CF_MMUENA;

  // wait for init to complete
#ifdef AVOID_DELAY
  while (!(PERIP(SDRAM_ST) & SDRAM_ST_INIT_DONE));
#else
  while (!(PERIP(SDRAM_ST) & SDRAM_ST_INIT_DONE)) Delay(1);
#endif
  
  /* Check whether there is data to be flushed in the read buffer. */
  SdRamReadCmd(0, 0);
#ifdef AVOID_DELAY
  DelayMicroSec(2000);
#else
  Delay(2);
#endif
  {
    int wordsWaiting;
    int printed = -1;
    while ((wordsWaiting =
	    (PERIP(SDRAM_ST) & SDRAM_ST_RX_32BIT_WORDS_AVAILABLE_MASK) >>
	    SDRAM_ST_RX_32BIT_WORDS_AVAILABLE_B)) {
      int i;
      if (printed == -1) {
	if (wordsWaiting != SDRAM_READ_LWORDS) {
	  printf("%d 32-bit words waiting in DDR instead of %d, flush!\n",
		 wordsWaiting, SDRAM_READ_LWORDS);
	}
	printed = wordsWaiting;
      }
      PERIP(SDRAM_DATA_HI);
      PERIP(SDRAM_DATA_LO);
    }
  }

  res = 0;
 finally:
  return res;
}



int WriteSdRam(register const u_int16 *s, register u_int32 addr, register u_int16 bytes, register s_int16 swapWords) {
  int res = -1;
  u_int32 wAddr = addr>>1;

  if ((bytes & 3) || (addr & 3)) goto finally;

  //  printf("#1: Start %lx %x\n", addr, bytes); Delay(1);
  if (bytes && (addr & 4)) {
    /* Address is divisible by 4, but not by 8, make a single transfer */
    if (SdRamWaitWriteBufEmpty()) goto finally;
    SdRamWriteCmd(wAddr, 1);
    PERIP(SDRAM_DATA_HI) = *s++;
    PERIP(SDRAM_DATA_LO) = *s++;
    bytes -= 4;
    wAddr += 2;
  }
  /* Bulk transfer, 32 bytes at the time. */
  //  printf("#2: Bulk\n"); Delay(1);
  if (bytes >= SDRAM_WRITE_BYTES) {
    int n = bytes/SDRAM_WRITE_BYTES;
#if 0
    if (2*wAddr >= 0xc59100 && 2*wAddr < 0xc59300) {
      static u_int16 buh[8];
      printf("##1 addr %08lx, current data at %08lx = ", 2*wAddr, 0xc59220);
      ReadSdRam(buh, 0xc59220, 16);
      printf("%x %x %x %x %x %x %x %x\n", buh[0], buh[1], buh[2], buh[3], buh[4], buh[5], buh[6], buh[7]);
    }
#endif

    if (SdRamWaitWriteBufEmpty()) goto finally;
    SdRamWriteCmd(wAddr, 0);
    if (swapWords) {
      if (SdRamWrite64NSwapWords(s, n)) goto finally;
    } else {
      if (SdRamWrite64N(s, n)) goto finally;
    }

#if 0
    if (2*wAddr >= 0xc59100 && 2*wAddr < 0xc59300) {
      static u_int16 buh[8];
      printf("##2 addr %08lx, current data at %08lx = ", 2*wAddr, 0xc59220);
      ReadSdRam(buh, 0xc59220, 16);
      printf("%x %x %x %x %x %x %x %x\n", buh[0], buh[1], buh[2], buh[3], buh[4], buh[5], buh[6], buh[7]);
    }
#endif

    bytes -= SDRAM_WRITE_BYTES*n;
    wAddr += (SDRAM_WRITE_BYTES/2)*n;
    s += (SDRAM_WRITE_BYTES/2)*n;
  }

  //  printf("#3: Tail 1\n"); Delay(1);
  /* Tail transfer #1: at least 8 bytes, 4 bytes at the time. */
  if (bytes >= 8) {
#if 0
    printf("Tail write #1, addr %08lx, bytes 0x%x\n", 2*wAddr, bytes);
#endif
    if (SdRamWaitWriteBufEmpty()) goto finally;
    SdRamWriteCmd(wAddr, 0);
    while (bytes >= 8) {
      if (swapWords) {
	PERIP(SDRAM_DATA_HI) = *s++;
	PERIP(SDRAM_DATA_LO) = *s++;
	PERIP(SDRAM_DATA_HI) = *s++;
	PERIP(SDRAM_DATA_LO) = *s++;
      } else {
	/* SDRAM_DATA_LO must be written last, hence the order! */
	PERIP(SDRAM_DATA_HI) = s[1];
	PERIP(SDRAM_DATA_LO) = s[0];
	PERIP(SDRAM_DATA_HI) = s[3];
	PERIP(SDRAM_DATA_LO) = s[2];
	s += 4;
      }
      bytes -= 8;
      wAddr += 4;
    }
  }
  //  printf("#4: Tail 2\n"); Delay(1);
  /* Tail transfer #2: 4 trailing bytes. */
  if (bytes) {
#if 0
    printf("Tail write #2, addr %08lx, bytes 0x%x = %02x %02x\n",
	   2*wAddr, bytes, s[0], s[1]);
#endif
    if (SdRamWaitWriteBufEmpty()) goto finally;
    SdRamWriteCmd(wAddr, 1);
    if (swapWords) {
      PERIP(SDRAM_DATA_HI) = *s++;
      PERIP(SDRAM_DATA_LO) = *s++;
    } else {
      /* SDRAM_DATA_LO must be written last, hence the order! */
      PERIP(SDRAM_DATA_HI) = s[1];
      PERIP(SDRAM_DATA_LO) = s[0];
      s += 2;
    }
    bytes -= 4;
    wAddr += 2;
  }
  //  printf("#5: Ready\n"); Delay(1);

  res = 0;
 finally:
  return res;
}



int ReadSdRam(register u_int16 *d, register u_int32 addr, register u_int16 bytes, register s_int16 swapWords) {
  int res = -1;
  u_int32 wAddr = addr>>1;

  if (bytes & 3) goto finally;

  if (SdRamWaitWriteBufEmpty()) goto finally;

  /* Bulk transfer, 32 bytes at the time. */
#if 1
  if (bytes >= 32) {
    int n = bytes/32;

#if 0
    while ((PERIP(SDRAM_ST) & SDRAM_ST_RX_32BIT_WORDS_AVAILABLE_MASK)) {
      printf("### Flush read, SDRAM_ST = %04x\n",
	     PERIP(SDRAM_ST));
      PERIP(SDRAM_DATA_HI);
      PERIP(SDRAM_DATA_LO);
    }
#endif

    if (swapWords) {
      SdRamRead32NSwapWords(wAddr, d, n);
    } else {
      SdRamRead32N(wAddr, d, n);
    }
    bytes -= 32*n;
    wAddr += 16*n;
    d += 16*n;
  }
#else
  while (bytes >= 32) {
    int i;
    SdRamReadCmd(wAddr, 0);
    if (SdRamWaitReadBufFilled()) goto finally;
    for (i=0; i<8; i++) {
      *d++ = PERIP(SDRAM_DATA_HI);
      *d++ = PERIP(SDRAM_DATA_LO);
    }
    bytes -= 32;
    wAddr += 16;
  }
#endif
  /* Tail transfer #1: at least 8 bytes, 4 bytes at the time. */
  if (bytes) {
    s_int16 bytesLeftInRead = 32;
    SdRamReadCmd(wAddr, 0);

    if (SdRamWaitReadBufFilled()) goto finally;
    while (bytes) {
      if (swapWords) {
	*d++ = PERIP(SDRAM_DATA_HI);
	*d++ = PERIP(SDRAM_DATA_LO);
      } else {
	/* SDRAM_DATA_LO must be read last, hence the order! */
	d[1] = PERIP(SDRAM_DATA_HI);
	d[0] = PERIP(SDRAM_DATA_LO);
	d += 2;
      }
      bytes -= 4;
      bytesLeftInRead -= 4;
      wAddr += 2;
    }
    /* Flush/ignore rest of data */
    while (bytesLeftInRead) {
      PERIP(SDRAM_DATA_HI);
      PERIP(SDRAM_DATA_LO);
      bytesLeftInRead -= 4;
    }
  }
#if 0
  /* Tail transfer #2: 4 trailing bytes. */
  if (bytes == 4) {
    SdRamReadCmd(wAddr, 1);
    if (SdRamWaitReadBufFilled()) goto finally;
    *d++ = PERIP(SDRAM_DATA_HI);
    *d++ = PERIP(SDRAM_DATA_LO);
    bytes -= 4;
    wAddr += 2;
  }
#endif

  res = 0;
 finally:
  return res;
}
