/*-----------------------------------------------------------------------*/
  /* MMCv3/SDv1/SDv2 (in native mode via MCI) control module (C)ChaN, 2010 */
  /*-----------------------------------------------------------------------*/
  /* This program is opened under license policy of following trems.
  /
  /  Copyright (C) 2010, ChaN, all right reserved.
  /
  / * This program is a free software and there is NO WARRANTY.
  / * No restriction on use. You can use, modify and redistribute it for
  /   personal, non-profit or commercial use UNDER YOUR RESPONSIBILITY.
  / * Redistributions of source code must retain the above copyright notice.
  /
  /---------------------------------------------------------------------------*/
  
  #include <string.h>
  #include "lpc23xx.h"
  #include "VIC.h"
  #include "xtimer.h"
  #include "diskio.h"
  
  #define ENABLE_DEBUG    (0)
  #include "debug.h"
  
  /* --- MCI configurations --- */
  #define N_BUF       4           /* Block transfer FIFO depth (>= 2) */
  #define USE_4BIT    1           /* Use wide bus mode if SDC is detected */
  #define PCLK        36000000UL  /* PCLK supplied to MCI module */
  #define MCLK_ID     400000UL    /* MCICLK for ID state (100k-400k) */
  #define MCLK_RW     18000000UL  /* MCICLK for data transfer (PCLK divided by even number) */
  
  /* This MCI driver assumes that MCLK_RW is CCLK/4 or slower. If block buffer underrun/overrun
  /  occured due to any interrupt by higher priority process or slow external memory, increasing
  /  N_BUF or decreasing MCLK_RW will solve it. */
  
  
  /* ----- Port definitions ----- */
  #define SOCKINS     !(FIO0PIN2 & 0x20)  /* Card detect switch */
  #define SOCKWP      (FIO0PIN2 & 0x04)   /* Write protect switch */
  
  
  /* ----- MMC/SDC command ----- */
  #define CMD0    (0)             /* GO_IDLE_STATE */
  #define CMD1    (1)             /* SEND_OP_COND (MMC) */
  #define CMD2    (2)             /* ALL_SEND_CID */
  #define CMD3    (3)             /* SEND_RELATIVE_ADDR */
  #define ACMD6   (6|0x80)        /* SET_BUS_WIDTH (SDC) */
  #define CMD7    (7)             /* SELECT_CARD */
  #define CMD8    (8)             /* SEND_IF_COND */
  #define CMD9    (9)             /* SEND_CSD */
  #define CMD10   (10)            /* SEND_CID */
  #define CMD12   (12)            /* STOP_TRANSMISSION */
  #define CMD13   (13)            /* SEND_STATUS */
  #define ACMD13  (13|0x80)       /* SD_STATUS (SDC) */
  #define CMD16   (16)            /* SET_BLOCKLEN */
  #define CMD17   (17)            /* READ_SINGLE_BLOCK */
  #define CMD18   (18)            /* READ_MULTIPLE_BLOCK */
  #define CMD23   (23)            /* SET_BLK_COUNT (MMC) */
  #define ACMD23  (23|0x80)       /* SET_WR_BLK_ERASE_COUNT (SDC) */
  #define CMD24   (24)            /* WRITE_BLOCK */
  #define CMD25   (25)            /* WRITE_MULTIPLE_BLOCK */
  #define CMD32   (32)            /* ERASE_ER_BLK_START */
  #define CMD33   (33)            /* ERASE_ER_BLK_END */
  #define CMD38   (38)            /* ERASE */
  #define ACMD41  (41|0x80)       /* SEND_OP_COND (SDC) */
  #define CMD55   (55)            /* APP_CMD */
  
  /* Card type flags (CardType) */
  #define CT_MMC      0x01        /* MMC ver 3 */
  #define CT_SD1      0x02        /* SD ver 1 */
  #define CT_SD2      0x04        /* SD ver 2 */
  #define CT_SDC      (CT_SD1|CT_SD2) /* SD */
  #define CT_BLOCK    0x08        /* Block addressing */
  
  
  /*--------------------------------------------------------------------------
  
     Module Private Functions
  
  ---------------------------------------------------------------------------*/
  
  static volatile diskio_sta_t Stat = DISKIO_STA_NOINIT;  /* Disk status */
  
  static unsigned short CardRCA;          /* Assigned RCA */
  static unsigned char CardType,          /* Card type flag */
         CardInfo[16 + 16 + 4];   /* CSD(16), CID(16), OCR(4) */
  
  static volatile unsigned char XferStat,             /* b3:MCI error, b2:Overrun, b1:Write, b0:Read */
         XferWc,          /* Write block counter */
         XferWp, XferRp;  /* R/W index of block FIFO */
  
  static unsigned long DmaBuff[N_BUF][128]  __attribute__((section(".usbdata")));  /* Block transfer FIFO */
  static unsigned long LinkList [N_BUF][4]  __attribute__((section(".usbdata")));  /* DMA link list */
  
  void Isr_MCI(void) __attribute__((interrupt("IRQ")));
  void Isr_GPDMA(void) __attribute__((interrupt("IRQ")));
  
  /*-----------------------------------------------------------------------*/
  /* Interrupt service routine for data transfer                           */
  /*-----------------------------------------------------------------------*/
  
  void Isr_MCI(void)
  {
      unsigned long ms;
      unsigned char xs;
  
      ms = MCI_STATUS & 0x073A;   /* Clear MCI interrupt status */
      MCI_CLEAR = ms;
  
      xs = XferStat;
  
      if (ms & 0x400) {           /* A block transfer completed (DataBlockEnd) */
          if (xs & 1) {               /* In card read operation */
              if (ms & 0x100) {           /* When last block is received (DataEnd), */
                  GPDMA_SOFT_BREQ = 0x10;    /* Pop off remaining data in the MCIFIFO */
              }
  
              unsigned char n = (XferWp + 1) % N_BUF;   /* Next write buffer */
              XferWp = n;
  
              if (n == XferRp) {
                  xs |= 4;    /* Check block overrun */
              }
          }
          else {                      /* In card write operation */
              unsigned char n = (XferRp + 1) % N_BUF;   /* Next read buffer */
              XferRp = n;
  
              if (n == XferWp) {
                  xs |= 4;    /* Check block underrun */
              }
          }
      }
      else {                  /* An MCI error occured (not DataBlockEnd) */
          xs |= 8;
      }
  
      XferStat = xs;
  
      VICVectAddr = 0;
  }
  
  void Isr_GPDMA(void)
  {
      if (GPDMA_INT_TCSTAT & BIT0) {
          GPDMA_INT_TCCLR = 0x01;             /* Clear GPDMA interrupt flag */
  
          if (XferStat & 2) {
              /* In write operation */
              if (--XferWc == N_BUF) { /* Terminate LLI */
                  LinkList[XferRp % N_BUF][2] = 0;
              }
          }
      }
      else {
          GPDMA_INT_TCCLR = 0x3;
      }
  
      VICVectAddr = 0;
  }
  
  
  /*-----------------------------------------------------------------------*/
  /* Ready for data reception                                              */
  /*-----------------------------------------------------------------------*/
  
  /**
   * @param blks          Number of blocks to receive (1..127)
   * @param bs            Block size (64 or 512)
   * */
  static void ready_reception(unsigned int blks, unsigned int bs)
  {
      unsigned int n;
      unsigned long dma_ctrl;
  
      /* ------ Setting up GPDMA Ch-0 ------ */
      GPDMA_CH0_CFG &= 0xFFF80420;        /* Disable ch-0 */
      GPDMA_INT_TCCLR = 0x01;             /* Clear interrupt flag */
      dma_ctrl = 0x88492000 | (bs / 4);   /* 1_000_1_0_00_010_010_010_010_************ */
  
      /* Create link list */
      for (n = 0; n < N_BUF; n++) {
          LinkList[n][0] = (unsigned long)&MCI_FIFO;
          LinkList[n][1] = (unsigned long)DmaBuff[n];
          LinkList[n][2] = (unsigned long)LinkList[(n + 1) % N_BUF];
          LinkList[n][3] = dma_ctrl;
      }
  
      /* Load first LLI */
      GPDMA_CH0_SRC = LinkList[0][0];
      GPDMA_CH0_DEST = LinkList[0][1];
      GPDMA_CH0_LLI = LinkList[0][2];
      GPDMA_CH0_CTRL = LinkList[0][3];
  
      /* Enable ch-0 */
      GPDMA_CH0_CFG |= 0x19009;           /* *************_0_0_1_1_0_010_*_0000_*_0100_1 */
  
      /* --------- Setting up MCI ---------- */
  
      XferRp = 0;
      XferWp = 0;                 /* Block FIFO R/W index */
      XferStat = 1;                           /* Transfer status: MCI --> Memory */
  
      MCI_DATA_LEN = bs * blks;               /* Set total data length */
      MCI_DATA_TMR = (unsigned long)(MCLK_RW * 0.2);  /* Data timer: 0.2sec */
      MCI_CLEAR = 0x72A;                      /* Clear status flags */
      MCI_MASK0 = 0x72A;                      /* DataBlockEnd StartBitErr DataEnd RxOverrun DataTimeOut DataCrcFail */
  
      for (n = 0; bs > 1; bs >>= 1, n += 0x10);
  
      MCI_DATA_CTRL  = n | 0xB;               /* Start to receive data blocks */
  }
  
  
  /*-----------------------------------------------------------------------*/
  /* Start to transmit a data block                                        */
  /*-----------------------------------------------------------------------*/
  
  #if _READONLY == 0
  /*
   * @param blks   Number of blocks to be transmitted (1..127)
   * */
  static void start_transmission(unsigned char blks)
  {
      unsigned int n;
      unsigned long dma_ctrl;
  
  
      /* ------ Setting up GPDMA Ch-0 ------ */
  
      GPDMA_CH0_CFG &= 0xFFF80420;        /* Disable ch-0 */
      GPDMA_INT_TCCLR = 0x01;             /* Clear interrupt flag */
      dma_ctrl = 0x84492080;              /* 1_000_0_1_00_010_010_010_010_000010000000 */
  
      /* Create link list */
      for (n = 0; n < N_BUF; n++) {
          LinkList[n][0] = (unsigned long)DmaBuff[n];
          LinkList[n][1] = (unsigned long)&MCI_FIFO;
          LinkList[n][2] = (n == (unsigned int) (blks - 1)) ? 0 : (unsigned long)LinkList[(n + 1) % N_BUF];
          LinkList[n][3] = dma_ctrl;
      }
  
      /* Load first LLI */
      GPDMA_CH0_SRC = LinkList[0][0];
      GPDMA_CH0_DEST = LinkList[0][1];
      GPDMA_CH0_LLI = LinkList[0][2];
      GPDMA_CH0_CTRL = LinkList[0][3];
  
      /* Enable ch-0 */
      GPDMA_CH0_CFG |= 0x18901;           /* *************_0_0_1_1_0_001_*_0100_*_0000_1 */
  
      /* --------- Setting up MCI ---------- */
  
      XferRp = 0;                             /* Block FIFO read index */
      XferWc = blks;
      XferStat = 2;                           /* Transfer status: Memroy --> MCI */
  
      MCI_DATA_LEN = 512 * (blks + 1);        /* Set total data length */
      MCI_DATA_TMR = (unsigned long)(MCLK_RW * 0.5);  /* Data timer: 0.5sec */
      MCI_CLEAR = 0x51A;                      /* Clear status flags */
      MCI_MASK0 = 0x51A;                      /* DataBlockEnd DataEnd TxUnderrun DataTimeOut DataCrcFail */
      MCI_DATA_CTRL  = (9 << 4) | 0x9;        /* Start to transmit data blocks */
  }
  #endif  /* _READONLY */
  
  
  
  
  /*-----------------------------------------------------------------------*/
  /* Stop data transfer                                                    */
  /*-----------------------------------------------------------------------*/
  
  static void stop_transfer(void)
  {
      MCI_MASK0 = 0;          /* Disable MCI interrupt */
      MCI_DATA_CTRL = 0;      /* Stop MCI data transfer */
  
      GPDMA_CH0_CFG &= 0xFFF80420;    /* Disable DMA ch-0 */
  }
  
  
  
  
  /*-----------------------------------------------------------------------*/
  /* Power Control (Device dependent)                                      */
  /*-----------------------------------------------------------------------*/
  
  static int power_status(void)
  {
      return (MCI_POWER & 3) ? 1 : 0;
  }
  
  
  static void power_on(void)
  {
      /* Enable MCI and GPDMA clock */
      PCONP |= (3 << 28);
  
      /* Enable GPDMA controller with little-endian */
      GPDMA_CH0_CFG &= 0xFFF80000;    /* Disable DMA ch-0 */
      GPDMA_CONFIG = 0x01;
  
      /* Select PCLK for MCI, CCLK/2 = 36MHz */
      PCLKSEL1 = (PCLKSEL1 & 0xFCFFFFFF) | 0x02000000;
  
      //0.19 0.20 0.21 0.22
      PINMODE1 &= ~((BIT6 | BIT7) | (BIT8 | BIT9) | (BIT10 | BIT11) | (BIT12 | BIT13));
      PINMODE1 |= (BIT7) | (BIT9) | (BIT11) | (BIT13);    // no resistors
      //2.11 2.12 2.13
      PINMODE4 &= ~((BIT22 | BIT23) | (BIT24 | BIT25) | (BIT26 | BIT27));
      PINMODE4 |= (BIT23) | (BIT25) | (BIT27);            // no resistors
      /* Attach MCI unit to I/O pad */
      PINSEL1 = (PINSEL1 & 0xFFFFC03F) | 0x00002A80;  /* MCICLK, MCICMD, MCIDATA0, MCIPWR */
  #if USE_4BIT
      PINSEL4 = (PINSEL4 & 0xF03FFFFF) | 0x0A800000;  /* MCIDATA1-3 */
  #endif
      MCI_MASK0 = 0;
      MCI_COMMAND = 0;
      MCI_DATA_CTRL = 0;
  
      // pin 0.21 high active
      SCS |= 0x08;
  
      /* Register interrupt handlers for MCI,DMA event */
      //RegisterIrq(MCI_INT, Isr_MCI, PRI_LOWEST-1);
      install_irq(MCI_INT, Isr_MCI, 5);
  
      //RegisterIrq(GPDMA_INT, Isr_GPDMA, PRI_LOWEST-1);
      install_irq(GPDMA_INT, Isr_GPDMA, 5);
  
  
      /* Power-on (VCC is always tied to the socket on this board) */
      MCI_POWER = 0x01;                   /* Power on */
  
      //for (Timer[0] = 10; Timer[0]; ) ; /* 10ms */
      xtimer_usleep(1000);
  
      MCI_POWER = 0x03;                   /* Enable signals */
  }
  
  
  static void power_off(void)
  {
      MCI_MASK0 = 0;
      MCI_COMMAND = 0;
      MCI_DATA_CTRL = 0;
  
      MCI_POWER = 0;          /* Power-off */
      MCI_CLOCK = 0;
  
      // pin 0.21 low inactive
      SCS &= ~0x08;
  
      //0.21 MCI led Pin (turns sd card off, too)
      //0.19 0.20 0.21 0.22 with pull-down
      PINMODE1 |= (BIT6 | BIT7) | (BIT8 | BIT9) | (BIT10 | BIT11) | (BIT12 | BIT13);
      PINSEL1 &= ~((BIT6 | BIT7) | (BIT8 | BIT9) | (BIT10 | BIT11) | (BIT12 | BIT13));
      //  Pins should be now configured as standard input (see board_init.c if you accidentally reconfigured them)
  
      //2.11 2.12 2.13 with pull-down
      PINMODE4 |= (BIT22 | BIT23) | (BIT24 | BIT25) | (BIT26 | BIT27);
      PINSEL4 &= ~((BIT22 | BIT23) | (BIT24 | BIT25) | (BIT26 | BIT27));
      //  Pins should be now configured as standard input (see board_init.c if you accidentally reconfigured them)
  
      Stat |= DISKIO_STA_NOINIT;
  }
  
  
  /*-----------------------------------------------------------------------*/
  /* Send a command packet to the card and receive a response              */
  /*-----------------------------------------------------------------------*/
  
  /*
   * @param idx       Command index (bit[5..0]), ACMD flag (bit7)
   * @param arg       Command argument
   * @param rt        Expected response type. None(0), Short(1) or Long(2)
   * @param *buff     Response return buffer
   * @return 1 when function succeeded otherwise returns 0
   * */
  static int send_cmd(unsigned int idx, unsigned long arg, unsigned int rt, unsigned long *buff)
  {
      unsigned int s, mc;
  
      if (idx & 0x80) {               /* Send a CMD55 prior to the specified command if it is ACMD class */
          if (!send_cmd(CMD55, (unsigned long)CardRCA << 16, 1, buff) /* When CMD55 is faild, */
             || !(buff[0] & 0x00000020)) {
              return 0;    /* exit with error */
          }
      }
  
      idx &= 0x3F;                    /* Mask out ACMD flag */
  
      do {                            /* Wait while CmdActive bit is set */
          MCI_COMMAND = 0;            /* Cancel to transmit command */
          MCI_CLEAR = 0x0C5;          /* Clear status flags */
  
          for (s = 0; s < 10; s++) {
              MCI_STATUS;    /* Skip lock out time of command reg. */
          }
      }
      while (MCI_STATUS & 0x00800);
  
      MCI_ARGUMENT = arg;             /* Set the argument into argument register */
      mc = 0x400 | idx;               /* Enable bit + index */
  
      if (rt == 1) {
          mc |= 0x040;    /* Set Response bit to reveice short resp */
      }
  
      if (rt > 1) {
          mc |= 0x0C0;    /* Set Response and LongResp bit to receive long resp */
      }
  
      MCI_COMMAND = mc;               /* Initiate command transaction */
  
      //Timer[1] = 100;
      uint32_t timerstart = xtimer_now_usec();
  
      while (1) {                     /* Wait for end of the cmd/resp transaction */
  
          //if (!Timer[1]) return 0;
          if ((xtimer_now_usec() - timerstart) > 10000) {
              return 0;
          }
  
          s = MCI_STATUS;             /* Get the transaction status */
  
          if (rt == 0) {
              if (s & 0x080) {
                  return 1;    /* CmdSent */
              }
          }
          else {
              if (s & 0x040) {
                  break;    /* CmdRespEnd */
              }
  
              if (s & 0x001) {
                  /* CmdCrcFail */
                  if (idx == 1 || idx == 12 || idx == 41) { /* Ignore CRC error on CMD1/12/41 */
                      break;
                  }
  
                  return 0;
              }
  
              if (s & 0x004) {
                  return 0;    /* CmdTimeOut */
              }
          }
      }
  
      buff[0] = MCI_RESP0;            /* Read the response words */
  
      if (rt == 2) {
          buff[1] = MCI_RESP1;
          buff[2] = MCI_RESP2;
          buff[3] = MCI_RESP3;
      }
  
      return 1;       /* Return with success */
  }
  
  
  
  
  /*-----------------------------------------------------------------------*/
  /* Wait card ready                                                       */
  /*-----------------------------------------------------------------------*/
  
  /**
   * @param tmr   Timeout in unit of 1ms
   * @returns 1 when card is tran state, otherwise returns 0
   */
  static int wait_ready(unsigned short tmr)
  {
      unsigned long rc;
  
      uint32_t stoppoll = xtimer_now_usec() + tmr * MS_IN_USEC;
      bool bBreak = false;
  
      while (xtimer_now_usec() < stoppoll/*Timer[0]*/) {
          if (send_cmd(CMD13, (unsigned long) CardRCA << 16, 1, &rc) && ((rc & 0x01E00) == 0x00800)) {
              bBreak = true;
              break;
          }
  
          /* This loop will take a time. Insert rot_rdq() here for multitask envilonment. */
      }
  
      return bBreak;//Timer[0] ? 1 : 0;
  }
  
  /*-----------------------------------------------------------------------*/
  /* Swap byte order                                                       */
  /*-----------------------------------------------------------------------*/
  static void bswap_cp(unsigned char *dst, const unsigned long *src)
  {
      unsigned long d;
  
  
      d = *src;
      *dst++ = (unsigned char)(d >> 24);
      *dst++ = (unsigned char)(d >> 16);
      *dst++ = (unsigned char)(d >> 8);
      *dst++ = (unsigned char)(d >> 0);
  }
  
  /*--------------------------------------------------------------------------
  
     Public Functions
  
  ---------------------------------------------------------------------------*/
  
  /*-----------------------------------------------------------------------*/
  /* Initialize Disk Drive                                                 */
  /*-----------------------------------------------------------------------*/
  diskio_sta_t mci_initialize(void)
  {
      unsigned int cmd, n;
      unsigned long resp[4];
      unsigned char ty;
  
      if (Stat & DISKIO_STA_NODISK) {
          return Stat;    /* No card in the socket */
      }
  
      power_off();
  
      xtimer_usleep(1000);
  
      power_on();                             /* Force socket power on */
      MCI_CLOCK = 0x100 | (PCLK / MCLK_ID / 2 - 1);   /* Set MCICLK = MCLK_ID */
      //for (Timer[0] = 2; Timer[0]; );
      xtimer_usleep(250);
  
      send_cmd(CMD0, 0, 0, NULL);             /* Enter idle state */
      CardRCA = 0;
  
      /*---- Card is 'idle' state ----*/
  
      /* Initialization timeout of 1000 msec */
      uint32_t start = xtimer_now_usec();
  
      /* SDC Ver2 */
      if (send_cmd(CMD8, 0x1AA, 1, resp) && (resp[0] & 0xFFF) == 0x1AA) {
          /* The card can work at vdd range of 2.7-3.6V */
          DEBUG("SDC Ver. 2\n");
  
          do {
              /* Wait while card is busy state (use ACMD41 with HCS bit) */
              /* This loop will take a time. Insert wai_tsk(1) here for multitask envilonment. */
              if (xtimer_now_usec() > (start + 1000000/* !Timer[0] */)) {
                  DEBUG("%s, %d: Timeout #1\n", RIOT_FILE_RELATIVE, __LINE__);
                  goto di_fail;
              }
          }
          while (!send_cmd(ACMD41, 0x40FF8000, 1, resp) || !(resp[0] & 0x80000000));
  
          ty = (resp[0] & 0x40000000) ? CT_SD2 | CT_BLOCK : CT_SD2;   /* Check CCS bit in the OCR */
      }
      else {                                  /* SDC Ver1 or MMC */
          if (send_cmd(ACMD41, 0x00FF8000, 1, resp)) {
              DEBUG("SDC Ver. 1\n");
              ty = CT_SD1;
              cmd = ACMD41;           /* ACMD41 is accepted -> SDC Ver1 */
          }
          else {
              DEBUG("MMC\n");
              ty = CT_MMC;
              cmd = CMD1;         /* ACMD41 is rejected -> MMC */
          }
  
          do {
              /* Wait while card is busy state (use ACMD41 or CMD1) */
              DEBUG("%s, %d: %lX\n", RIOT_FILE_RELATIVE, __LINE__, resp[0]);
  
              /* This loop will take a time. Insert wai_tsk(1) here for multitask envilonment. */
              if (xtimer_now_usec() > (start + 1000000/* !Timer[0] */)) {
                  DEBUG("now: %lu, started at: %lu\n", xtimer_now_usec(), start);
                  DEBUG("%s, %d: Timeout #2\n", RIOT_FILE_RELATIVE, __LINE__);
                  goto di_fail;
              }
          }
          while (!send_cmd(cmd, 0x00FF8000, 1, resp) || !(resp[0] & 0x80000000));
      }
  
      CardType = ty;                          /* Save card type */
      bswap_cp(&CardInfo[32], resp);          /* Save OCR */
  
      /*---- Card is 'ready' state ----*/
  
      if (!send_cmd(CMD2, 0, 2, resp)) {
          DEBUG("%s, %d: Failed entering ident state", RIOT_FILE_RELATIVE,
                __LINE__);
          goto di_fail;   /* Enter ident state */
      }
  
      for (n = 0; n < 4; n++) {
          bswap_cp(&CardInfo[n * 4 + 16], &resp[n]);    /* Save CID */
      }
  
      /*---- Card is 'ident' state ----*/
  
      if (ty & CT_SDC) {                      /* SDC: Get generated RCA and save it */
          if (!send_cmd(CMD3, 0, 1, resp)) {
              DEBUG("%s, %d: Failed generating RCA\n", RIOT_FILE_RELATIVE, __LINE__);
              goto di_fail;
          }
  
          CardRCA = (unsigned short)(resp[0] >> 16);
      }
      else {                              /* MMC: Assign RCA to the card */
          if (!send_cmd(CMD3, 1 << 16, 1, resp)) {
              goto di_fail;
          }
  
          CardRCA = 1;
      }
  
      /*---- Card is 'stby' state ----*/
  
      if (!send_cmd(CMD9, (unsigned long)CardRCA << 16, 2, resp)) {   /* Get CSD and save it */
          goto di_fail;
      }
  
      for (n = 0; n < 4; n++) {
          bswap_cp(&CardInfo[n * 4], &resp[n]);
      }
  
      if (!send_cmd(CMD7, (unsigned long)CardRCA << 16, 1, resp)) {   /* Select card */
          //printf("MCI CMD7 fail\n");
          goto di_fail;
      }
  
      /*---- Card is 'tran' state ----*/
  
      if (!(ty & CT_BLOCK)) {     /* Set data block length to 512 (for byte addressing cards) */
          if (!send_cmd(CMD16, 512, 1, resp) || (resp[0] & 0xFDF90000)) {
              //printf("MCI CMD16 fail\n");
              goto di_fail;
          }
      }
  
  #if USE_4BIT
  
      if (ty & CT_SDC) {      /* Set wide bus mode (for SDCs) */
          if (!send_cmd(ACMD6, 2, 1, resp)    /* Set bus mode of SDC */
             || (resp[0] & 0xFDF90000)) {
              //printf("MCI ACMD6 fail\n");
              goto di_fail;
          }
  
          MCI_CLOCK |= 0x800;                 /* Set bus mode of MCI */
      }
  
  #endif
  
      MCI_CLOCK = (MCI_CLOCK & 0xF00) | 0x200 | (PCLK / MCLK_RW / 2 - 1);     /* Set MCICLK = MCLK_RW, power-save mode */
  
      Stat &= ~DISKIO_STA_NOINIT;    /* Clear DISKIO_STA_NOINIT */
      return Stat;
  
  di_fail:
      power_off();
      Stat |= DISKIO_STA_NOINIT;     /* Set DISKIO_STA_NOINIT */
      return Stat;
  }
  
  
  
  
  /*-----------------------------------------------------------------------*/
  /* Get Disk Status                                                       */
  /*-----------------------------------------------------------------------*/
  
  diskio_sta_t mci_status(void)
  {
      return Stat;
  }
  
  
  
  
  /*-----------------------------------------------------------------------*/
  /* Read Sector(s)                                                        */
  /*-----------------------------------------------------------------------*/
  
  /**
   * @param buff      Pointer to the data buffer to store read data
   * @param sector    Start sector number (LBA)
   * @param count     Sector count (1..127)
   */
  diskio_result_t mci_read(unsigned char *buff, unsigned long sector, unsigned char count)
  {
      unsigned long resp;
      unsigned int cmd;
  
      if (count < 1 || count > 127) {
          return DISKIO_RES_PARERR;    /* Check parameter */
      }
  
      if (Stat & DISKIO_STA_NOINIT) {
          return DISKIO_RES_NOTRDY;    /* Check drive status */
      }
  
      if (!(CardType & CT_BLOCK)) {
          sector *= 512;    /* Convert LBA to byte address if needed */
      }
  
      if (!wait_ready(500)) {
          return DISKIO_RES_ERROR;    /* Make sure that card is tran state */
      }
  
      ready_reception(count, 512);            /* Ready to receive data blocks */
  
      cmd = (count > 1) ? CMD18 : CMD17;      /* Transfer type: Single block or Multiple block */
  
      if (send_cmd(cmd, sector, 1, &resp)     /* Start to read */
         && !(resp & 0xC0580000)) {
          unsigned char rp = 0;
  
          do {
              while ((rp == XferWp) && !(XferStat & 0xC)) {
                  /* Wait for block arrival */
                  /* This loop will take a time. Replace it with sync process for multitask envilonment. */
              }
  
              if (XferStat & 0xC) {
                  break; /* Abort if any error has occured */
              }
  
              copy_al2un(buff, DmaBuff[rp], 512); /* Pop an block */
  
              XferRp = rp = (rp + 1) % N_BUF; /* Next DMA buffer */
  
              if (XferStat & 0xC) {
                  break; /* Abort if overrun has occured */
              }
  
              buff += 512; /* Next user buffer address */
          } while (--count);
  
          if (cmd == CMD18) { /* Terminate to read (MB) */
              send_cmd(CMD12, 0, 1, &resp);
          }
      }
  
      stop_transfer(); /* Close data path */
  
      return count ? DISKIO_RES_ERROR : DISKIO_RES_OK;
  }
  
  
  /*-----------------------------------------------------------------------*/
  /* Write Sector(s)                                                       */
  /*-----------------------------------------------------------------------*/
  
  #if _READONLY == 0
  
  /**
   * @param buff     Pointer to the data to be written
   * @param sector   Start sector number (LBA)
   * @param count    Sector count (1..127)
   * */
  diskio_result_t mci_write(const unsigned char *buff, unsigned long sector, unsigned char count)
  {
      unsigned long rc;
      unsigned int cmd;
      unsigned char wp, xc;
  
      if (count < 1 || count > 127) {
          return DISKIO_RES_PARERR;    /* Check parameter */
      }
  
      if (Stat & DISKIO_STA_NOINIT) {
          return DISKIO_RES_NOTRDY;    /* Check drive status */
      }
  
      if (Stat & DISKIO_STA_PROTECT) {
          return DISKIO_RES_WRPRT;    /* Check write protection */
      }
  
      if (!(CardType & CT_BLOCK)) {
          sector *= 512;    /* Convert LBA to byte address if needed */
      }
  
      if (!wait_ready(500)) {
          return DISKIO_RES_ERROR;    /* Make sure that card is tran state */
      }
  
      if (count == 1) {   /* Single block write */
          cmd = CMD24;
      }
      else {          /* Multiple block write */
          cmd = (CardType & CT_SDC) ? ACMD23 : CMD23;
  
          if (!send_cmd(cmd, count, 1, &rc)       /* Preset number of blocks to write */
             || (rc & 0xC0580000)) {
              return DISKIO_RES_ERROR;
          }
  
          cmd = CMD25;
      }
  
      if (!send_cmd(cmd, sector, 1, &rc)          /* Send a write command */
         || (rc & 0xC0580000)) {
          return DISKIO_RES_ERROR;
      }
  
      wp = 0;
      xc = count;
  
      do {                                        /* Fill block FIFO */
          copy_un2al(DmaBuff[wp], (unsigned char *)(unsigned int)buff, 512);  /* Push a block */
          wp++;                                       /* Next DMA buffer */
          count--;
          buff += 512;                        /* Next user buffer address */
      }
      while (count && wp < N_BUF);
  
      XferWp = wp = wp % N_BUF;
      start_transmission(xc);                     /* Start transmission */
  
      while (count) {
          while ((wp == XferRp) && !(XferStat & 0xC)) {   /* Wait for block FIFO not full */
              /* This loop will take a time. Replace it with sync process for multitask envilonment. */
          }
  
          if (XferStat & 0xC) {
              break;    /* Abort if block underrun or any MCI error has occured */
          }
  
          copy_un2al(DmaBuff[wp], (unsigned char *)(unsigned int)buff, 512);  /* Push a block */
          XferWp = wp = (wp + 1) % N_BUF;             /* Next DMA buffer */
  
          if (XferStat & 0xC) {
              break;    /* Abort if block underrun has occured */
          }
  
          count--;
          buff += 512;             /* Next user buffer address */
      }
  
      while (!(XferStat & 0xC)) {} /* Wait for all blocks sent (block underrun) */
  
      if (XferStat & 0x8) {
          count = 1;    /* Abort if any MCI error has occured */
      }
  
      stop_transfer();                            /* Close data path */
  
      if (cmd == CMD25 && (CardType & CT_SDC)) {  /* Terminate to write (SDC w/MB) */
          send_cmd(CMD12, 0, 1, &rc);
      }
  
      return count ? DISKIO_RES_ERROR : DISKIO_RES_OK;
  }
  #endif /* _READONLY */
  
  
  
  
  /*-----------------------------------------------------------------------*/
  /* Miscellaneous Functions                                               */
  /*-----------------------------------------------------------------------*/
  
  diskio_result_t mci_ioctl(
      unsigned char ctrl,     /* Control code */
      void *buff      /* Buffer to send/receive data block */
  )
  {
      diskio_result_t res;
      unsigned char *ptr = (unsigned char *)buff;
      unsigned long resp[4], d, *dp, st, ed;
  
  
      if (Stat & DISKIO_STA_NOINIT) {
          return DISKIO_RES_NOTRDY;
      }
  
      res = DISKIO_RES_ERROR;
  
      switch(ctrl) {
          case CTRL_SYNC :    /* Make sure that all data has been written on the media */
              if (wait_ready(500)) {  /* Wait for card enters tarn state */
                  res = DISKIO_RES_OK;
              }
  
              break;
  
          case GET_SECTOR_COUNT : /* Get number of sectors on the disk (unsigned long) */
              if ((CardInfo[0] >> 6) == 1) {  /* SDC CSD v2.0 */
                  d = ((unsigned short)CardInfo[8] << 8) + CardInfo[9] + 1;
                  *(unsigned long *)buff = d << 10;
              }
              else {                      /* MMC or SDC CSD v1.0 */
                  unsigned char b = (CardInfo[5] & 15) + ((CardInfo[10] & 128) >> 7) + ((CardInfo[9] & 3) << 1) + 2;
                  d = (CardInfo[8] >> 6) + ((unsigned short)CardInfo[7] << 2) + ((unsigned short)(CardInfo[6] & 3) << 10) + 1;
                  *(unsigned long *)buff = d << (b - 9);
              }
  
              res = DISKIO_RES_OK;
              break;
  
          case GET_SECTOR_SIZE :  /* Get sectors on the disk (unsigned short) */
              *(unsigned short *)buff = 512;
              res = DISKIO_RES_OK;
              break;
  
          case GET_BLOCK_SIZE :   /* Get erase block size in unit of sectors (unsigned long) */
              if (CardType & CT_SD2) {    /* SDC ver 2.00 */
                  *(unsigned long *)buff = 16UL << (CardInfo[10] >> 4);
              }
              else {                  /* SDC ver 1.XX or MMC */
                  if (CardType & CT_SD1) {    /* SDC v1 */
                      *(unsigned long *)buff = (((CardInfo[10] & 63) << 1) + ((unsigned short)(CardInfo[11] & 128) >> 7) + 1) << ((CardInfo[13] >> 6) - 1);
                  }
                  else {              /* MMC */
                      *(unsigned long *)buff = ((unsigned short)((CardInfo[10] & 124) >> 2) + 1) * (((CardInfo[11] & 3) << 3) + ((CardInfo[11] & 224) >> 5) + 1);
                  }
              }
  
              res = DISKIO_RES_OK;
              break;
  
          case CTRL_ERASE_SECTOR :    /* Erase a block of sectors */
              if (!(CardType & CT_SDC) || (!(CardInfo[0] >> 6) && !(CardInfo[10] & 0x40))) {
                  break;    /* Check if sector erase can be applied to the card */
              }
  
              dp = (unsigned long *)buff;
              st = dp[0];
              ed = dp[1];
  
              if (!(CardType & CT_BLOCK)) {
                  st *= 512;
                  ed *= 512;
              }
  
              if (send_cmd(CMD32, st, 1, resp) && send_cmd(CMD33, ed, 1, resp) && send_cmd(CMD38, 0, 1, resp) && wait_ready(30000)) {
                  res = DISKIO_RES_OK;
              }
  
              break;
  
          case CTRL_POWER :
              switch(ptr[0]) {
                  case 0:     /* Sub control code == 0 (POWER_OFF) */
                      power_off();        /* Power off */
                      res = DISKIO_RES_OK;
                      break;
  
                  case 1:     /* Sub control code == 1 (POWER_GET) */
                      ptr[1] = (unsigned char)power_status();
                      res = DISKIO_RES_OK;
                      break;
  
                  default :
                      res = DISKIO_RES_PARERR;
              }
  
              break;
  
          case MMC_GET_TYPE :     /* Get card type flags (1 byte) */
              *ptr = CardType;
              res = DISKIO_RES_OK;
              break;
  
          case MMC_GET_CSD :      /* Get CSD (16 bytes) */
              memcpy(buff, &CardInfo[0], 16);
              res = DISKIO_RES_OK;
              break;
  
          case MMC_GET_CID :      /* Get CID (16 bytes) */
              memcpy(buff, &CardInfo[16], 16);
              res = DISKIO_RES_OK;
              break;
  
          case MMC_GET_OCR :      /* Get OCR (4 bytes) */
              memcpy(buff, &CardInfo[32], 4);
              res = DISKIO_RES_OK;
              break;
  
          case MMC_GET_SDSTAT :   /* Receive SD status as a data block (64 bytes) */
              if (CardType & CT_SDC) {    /* SDC */
                  if (wait_ready(500)) {
                      ready_reception(1, 64);             /* Ready to receive data blocks */
  
                      if (send_cmd(ACMD13, 0, 1, resp)    /* Start to read */
                         && !(resp[0] & 0xC0580000)) {
                          while ((XferWp == 0) && !(XferStat & 0xC)) {}
  
                          if (!(XferStat & 0xC)) {
                              copy_al2un((unsigned char *)buff, DmaBuff[0], 64);
                              res = DISKIO_RES_OK;
                          }
                      }
                  }
  
                  stop_transfer();                    /* Close data path */
              }
  
              break;
  
          default:
              res = DISKIO_RES_PARERR;
      }
  
      return res;
  }