text: #include <stdint.h>
  #include <kernel/emmc.h>
  #include <kernel/peripheral.h>
  #include <kernel/mailbox.h>
  #include <kernel/uart.h>
  #include <kernel/mem.h>
  #include <common/util.h>
  #include <common/stdlib.h>
  
  static void sd_power_off() {
      /* Power off the SD card */
      uint32_t control0 = mmio_read(EMMC_BASE + EMMC_CONTROL0);
      control0 &= ~(1 << 8);	// Set SD Bus Power bit off in Power Control Register
      mmio_write(EMMC_BASE + EMMC_CONTROL0, control0);
  }
  
  static uint32_t sd_get_base_clock_hz() {
      mail_message_t msg;
      uint32_t mailbuffer[8];
  
      /* Get the base clock rate */
      // set up the buffer
      mailbuffer[0] = 8 * 4;		// size of this message
      mailbuffer[1] = 0;			// this is a request
  
      // next comes the first tag
      mailbuffer[2] = 0x00030002;	// get clock rate tag
      mailbuffer[3] = 0x8;		// value buffer size
      mailbuffer[4] = 0x4;		// is a request, value length = 4
      mailbuffer[5] = 0x1;		// clock id + space to return clock id
      mailbuffer[6] = 0;			// space to return rate (in Hz)
  
      // closing tag
      mailbuffer[7] = 0;
  
      // send the message
      msg.data = (uint32_t)mailbuffer;
      mailbox_send(msg, PROPERTY_CHANNEL);
  
      // read the response
      msg = mailbox_read(PROPERTY_CHANNEL);
  
      if (!msg.data)
          return -1;
  
      if(mailbuffer[1] != MBOX_SUCCESS) {
          uart_printf("EMMC: property mailbox did not return a valid response.\n");
          return 0;
      }
  
      if(mailbuffer[5] != 0x1) {
          uart_printf("EMMC: property mailbox did not return a valid clock id.\n");
          return 0;
      }
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: base clock rate is %i Hz\n", mailbuffer[6]);
  #endif
      return mailbuffer[6];
  }
  
  static int bcm_2708_power_off() {
      mail_message_t msg;
      uint32_t mailbuffer[8];
  
      /* Power off the SD card */
      // set up the buffer
      mailbuffer[0] = 8 * 4;		// size of this message
      mailbuffer[1] = 0;			// this is a request
  
      // next comes the first tag
      mailbuffer[2] = 0x00028001;	// set power state tag
      mailbuffer[3] = 0x8;		// value buffer size
      mailbuffer[4] = 0x8;		// is a request, value length = 8
      mailbuffer[5] = 0x0;		// device id and device id also returned here
      mailbuffer[6] = 0x2;		// set power off, wait for stable and returns state
  
      // closing tag
      mailbuffer[7] = 0;
  
      // send the message
      msg.data = (uint32_t)mailbuffer;
      mailbox_send(msg, PROPERTY_CHANNEL);
  
      // read the response
      mailbox_read(PROPERTY_CHANNEL);
  
      if(mailbuffer[1] != MBOX_SUCCESS)
      {
          uart_printf("EMMC: bcm_2708_power_off(): property mailbox did not return a valid response.\n");
          return -1;
      }
  
      if(mailbuffer[5] != 0x0)
      {
          uart_printf("EMMC: property mailbox did not return a valid device id.\n");
          return -1;
      }
  
      if((mailbuffer[6] & 0x3) != 0)
      {
  #ifdef EMMC_DEBUG
          uart_printf("EMMC: bcm_2708_power_off(): device did not power off successfully (%08x).\n", mailbuffer[6]);
  #endif
          return 1;
      }
  
      return 0;
  }
  
  static int bcm_2708_power_on() {
      mail_message_t msg;
      uint32_t mailbuffer[8];
  
      /* Power on the SD card */
      // set up the buffer
      mailbuffer[0] = 8 * 4;		// size of this message
      mailbuffer[1] = 0;			// this is a request
  
      // next comes the first tag
      mailbuffer[2] = 0x00028001;	// set power state tag
      mailbuffer[3] = 0x8;		// value buffer size
      mailbuffer[4] = 0x8;		// is a request, value length = 8
      mailbuffer[5] = 0x0;		// device id and device id also returned here
      mailbuffer[6] = 0x3;		// set power off, wait for stable and returns state
  
      // closing tag
      mailbuffer[7] = 0;
  
      // send the message
      msg.data = (uint32_t)mailbuffer;
      mailbox_send(msg, PROPERTY_CHANNEL);
  
      // read the response
      mailbox_send(msg, PROPERTY_CHANNEL);
  
      if(mailbuffer[1] != MBOX_SUCCESS)
      {
          uart_printf("EMMC: bcm_2708_power_on(): property mailbox did not return a valid response.\n");
          return -1;
      }
  
      if(mailbuffer[5] != 0x0)
      {
          uart_printf("EMMC: property mailbox did not return a valid device id.\n");
          return -1;
      }
  
      if((mailbuffer[6] & 0x3) != 1)
      {
  #ifdef EMMC_DEBUG
          uart_printf("EMMC: bcm_2708_power_on(): device did not power on successfully (%08x).\n", mailbuffer[6]);
  #endif
          return 1;
      }
  
      return 0;
  }
  
  static int bcm_2708_power_cycle()
  {
      if(bcm_2708_power_off() < 0)
          return -1;
  
      udelay(5000);
  
      return bcm_2708_power_on();
  }
  
  // Set the clock dividers to generate a target value
  static uint32_t sd_get_clock_divider(uint32_t base_clock, uint32_t target_rate) {
      uint32_t targetted_divisor = 0;
      if (target_rate > base_clock) {
          targetted_divisor = 1;
      } else {
          divmod_t mod = divmod(base_clock, target_rate);
          targetted_divisor = mod.div;
          if(mod.mod)
              targetted_divisor--;
      }
  
      if(hci_ver >= 2) {
          // HCI version 3 or greater supports 10-bit divided clock mode
          // This requires a power-of-two divider
  
          // Find the first bit set
          int divisor = -1;
          for (int first_bit = 31; first_bit >= 0; first_bit--) {
              uint32_t bit_test = (1 << first_bit);
              if(targetted_divisor & bit_test) {
                  divisor = first_bit;
                  targetted_divisor &= ~bit_test;
                  if(targetted_divisor) {
                      // The divisor is not a power-of-two, increase it
                      divisor++;
                  }
                  break;
              }
          }
  
          if(divisor == -1 || divisor >= 32)
              divisor = 31;
  
          if(divisor != 0)
              divisor = (1 << (divisor - 1));
  
          if(divisor >= 0x400)
              divisor = 0x3ff;
  
          uint32_t freq_select = divisor & 0xff;
          uint32_t upper_bits = (divisor >> 8) & 0x3;
          uint32_t ret = (freq_select << 8) | (upper_bits << 6) | (0 << 5);
  
  #ifdef EMMC_DEBUG
          int denominator = 1;
              if(divisor != 0)
                  denominator = divisor * 2;
              int actual_clock = div(base_clock, denominator);
              uart_printf("EMMC: base_clock: %i, target_rate: %i, divisor: %08x, "
                     "actual_clock: %i, ret: %08x\n", base_clock, target_rate,
                     divisor, actual_clock, ret);
  #endif
  
          return ret;
      } else {
          uart_printf("EMMC: unsupported host version\n");
          return SD_GET_CLOCK_DIVIDER_FAIL;
      }
  }
  
  // Switch the clock rate whilst running
  static int sd_switch_clock_rate(uint32_t base_clock, uint32_t target_rate) {
      // Decide on an appropriate divider
      uint32_t divider = sd_get_clock_divider(base_clock, target_rate);
      if(divider == SD_GET_CLOCK_DIVIDER_FAIL) {
          uart_printf("EMMC: couldn't get a valid divider for target rate %i Hz\n", target_rate);
          return -1;
      }
  
      // Wait for the command inhibit (CMD and DAT) bits to clear
      while(mmio_read(EMMC_BASE + EMMC_STATUS) & 0x3)
          udelay(1000);
  
      // Set the SD clock off
      uint32_t control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
      control1 &= ~(1 << 2);
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      udelay(2000);
  
      // Write the new divider
      control1 &= ~0xffe0; // Clear old setting + clock generator select
      control1 |= divider;
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      udelay(2000);
  
      // Enable the SD clock
      control1 |= (1 << 2);
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      udelay(2000);
  
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: successfully set clock rate to %i Hz\n", target_rate);
  #endif
      return 0;
  }
  
  // Reset the CMD line
  static int sd_reset_cmd() {
      uint32_t control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
      control1 |= SD_RESET_CMD;
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      TIMEOUT_WAIT((mmio_read(EMMC_BASE + EMMC_CONTROL1) & SD_RESET_CMD) == 0, 1000000);
      if((mmio_read(EMMC_BASE + EMMC_CONTROL1) & SD_RESET_CMD) != 0) {
          uart_printf("EMMC: CMD line did not reset properly\n");
          return -1;
      }
      return 0;
  }
  
  // Reset the CMD line
  static int sd_reset_dat() {
      uint32_t control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
      control1 |= SD_RESET_DAT;
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      TIMEOUT_WAIT((mmio_read(EMMC_BASE + EMMC_CONTROL1) & SD_RESET_DAT) == 0, 1000000);
      if((mmio_read(EMMC_BASE + EMMC_CONTROL1) & SD_RESET_DAT) != 0) {
          uart_printf("EMMC: DAT line did not reset properly\n");
          return -1;
      }
      return 0;
  }
  
  static void sd_issue_command_int(struct emmc_block_dev *dev, uint32_t cmd_reg, uint32_t argument, useconds_t timeout) {
      dev->last_cmd_reg = cmd_reg;
      dev->last_cmd_success = 0;
  
      // This is as per HCSS 3.7.1.1/3.7.2.2
      // Check Command Inhibit
      while(mmio_read(EMMC_BASE + EMMC_STATUS) & 0x1)
          udelay(1000);
  
      // Is the command with busy?
      if((cmd_reg & SD_CMD_RSPNS_TYPE_MASK) == SD_CMD_RSPNS_TYPE_48B) {
          // With busy
          // Is is an abort command?
          if((cmd_reg & SD_CMD_TYPE_MASK) != SD_CMD_TYPE_ABORT) {
              // Not an abort command
              // Wait for the data line to be free
              while(mmio_read(EMMC_BASE + EMMC_STATUS) & 0x2)
                  udelay(1000);
          }
      }
  
      // Is this a DMA transfer?
      int is_sdma = 0;
      if((cmd_reg & SD_CMD_ISDATA) && (dev->use_sdma)) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: performing SDMA transfer, current INTERRUPT: %08x\n", mmio_read(EMMC_BASE + EMMC_INTERRUPT));
  #endif
          is_sdma = 1;
      }
  
      // Set block size and block count
      // For now, block size = 512 bytes, block count = 1,
      //  host SDMA buffer boundary = 4 kiB
      if(dev->blocks_to_transfer > 0xffff) {
          uart_printf("SD: blocks_to_transfer too great (%i)\n", dev->blocks_to_transfer);
          dev->last_cmd_success = 0;
          return;
      }
      uint32_t blksizecnt = dev->block_size | (dev->blocks_to_transfer << 16);
      mmio_write(EMMC_BASE + EMMC_BLKSIZECNT, blksizecnt);
  
      // Set argument 1 reg
      mmio_write(EMMC_BASE + EMMC_ARG1, argument);
  
      if(is_sdma) {
          // Set Transfer mode register
          cmd_reg |= SD_CMD_DMA;
      }
  
      // Set command reg
      mmio_write(EMMC_BASE + EMMC_CMDTM, cmd_reg);
  
      udelay(2000);
  
      // Wait for command complete interrupt
      TIMEOUT_WAIT(mmio_read(EMMC_BASE + EMMC_INTERRUPT) & 0x8001, timeout);
      uint32_t irpts = mmio_read(EMMC_BASE + EMMC_INTERRUPT);
  
      // Clear command complete status
      mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff0001);
  
      // Test for errors
      if((irpts & 0xffff0001) != 0x1) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: error occured whilst waiting for command complete interrupt\n");
  #endif
          dev->last_error = irpts & 0xffff0000;
          dev->last_interrupt = irpts;
          return;
      }
  
      udelay(2000);
  
      // Get response data
      switch(cmd_reg & SD_CMD_RSPNS_TYPE_MASK) {
          case SD_CMD_RSPNS_TYPE_48:
          case SD_CMD_RSPNS_TYPE_48B:
              dev->last_r0 = mmio_read(EMMC_BASE + EMMC_RESP0);
              break;
  
          case SD_CMD_RSPNS_TYPE_136:
              dev->last_r0 = mmio_read(EMMC_BASE + EMMC_RESP0);
              dev->last_r1 = mmio_read(EMMC_BASE + EMMC_RESP1);
              dev->last_r2 = mmio_read(EMMC_BASE + EMMC_RESP2);
              dev->last_r3 = mmio_read(EMMC_BASE + EMMC_RESP3);
              break;
      }
  
      // If with data, wait for the appropriate interrupt
      if((cmd_reg & SD_CMD_ISDATA) && (is_sdma == 0)) {
          uint32_t wr_irpt;
          int is_write = 0;
          if(cmd_reg & SD_CMD_DAT_DIR_CH)
              wr_irpt = (1 << 5);     // read
          else {
              is_write = 1;
              wr_irpt = (1 << 4);     // write
          }
  
          int cur_block = 0;
          uint32_t *cur_buf_addr = (uint32_t *)dev->buf;
          while(cur_block < dev->blocks_to_transfer) {
  #ifdef EMMC_DEBUG
              if(dev->blocks_to_transfer > 1)
  				uart_printf("SD: multi block transfer, awaiting block %i ready\n", cur_block);
  #endif
              TIMEOUT_WAIT(mmio_read(EMMC_BASE + EMMC_INTERRUPT) & (wr_irpt | 0x8000), timeout);
              irpts = mmio_read(EMMC_BASE + EMMC_INTERRUPT);
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff0000 | wr_irpt);
  
              if((irpts & (0xffff0000 | wr_irpt)) != wr_irpt) {
  #ifdef EMMC_DEBUG
                  uart_printf("SD: error occured whilst waiting for data ready interrupt\n");
  #endif
                  dev->last_error = irpts & 0xffff0000;
                  dev->last_interrupt = irpts;
                  return;
              }
  
              // Transfer the block
              uint64_t cur_byte_no = 0;
              while(cur_byte_no < dev->block_size) {
                  if(is_write) {
                      uint32_t data = read_word((uint8_t *)cur_buf_addr, 0);
                      mmio_write(EMMC_BASE + EMMC_DATA, data);
                  } else {
                      uint32_t data = mmio_read(EMMC_BASE + EMMC_DATA);
                      write_word(data, (uint8_t *)cur_buf_addr, 0);
                  }
                  cur_byte_no += 4;
                  cur_buf_addr++;
              }
  #ifdef EMMC_DEBUG
              uart_printf("SD: block %i transfer complete\n", cur_block);
  #endif
              cur_block++;
          }
      }
  
      // Wait for transfer complete (set if read/write transfer or with busy)
      if((((cmd_reg & SD_CMD_RSPNS_TYPE_MASK) == SD_CMD_RSPNS_TYPE_48B) ||
          (cmd_reg & SD_CMD_ISDATA)) && (is_sdma == 0)) {
          // First check command inhibit (DAT) is not already 0
          if((mmio_read(EMMC_BASE + EMMC_STATUS) & 0x2) == 0)
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff0002);
          else {
              TIMEOUT_WAIT(mmio_read(EMMC_BASE + EMMC_INTERRUPT) & 0x8002, timeout);
              irpts = mmio_read(EMMC_BASE + EMMC_INTERRUPT);
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff0002);
  
              // Handle the case where both data timeout and transfer complete
              //  are set - transfer complete overrides data timeout: HCSS 2.2.17
              if(((irpts & 0xffff0002) != 0x2) && ((irpts & 0xffff0002) != 0x100002)) {
  #ifdef EMMC_DEBUG
                  uart_printf("SD: error occured whilst waiting for transfer complete interrupt\n");
  #endif
                  dev->last_error = irpts & 0xffff0000;
                  dev->last_interrupt = irpts;
                  return;
              }
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff0002);
          }
      } else if (is_sdma) {
          // For SDMA transfers, we have to wait for either transfer complete,
          //  DMA int or an error
  
          // First check command inhibit (DAT) is not already 0
          if((mmio_read(EMMC_BASE + EMMC_STATUS) & 0x2) == 0)
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff000a);
          else {
              TIMEOUT_WAIT(mmio_read(EMMC_BASE + EMMC_INTERRUPT) & 0x800a, timeout);
              irpts = mmio_read(EMMC_BASE + EMMC_INTERRUPT);
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffff000a);
  
              // Detect errors
              if((irpts & 0x8000) && ((irpts & 0x2) != 0x2)) {
  #ifdef EMMC_DEBUG
                  uart_printf("SD: error occured whilst waiting for transfer complete interrupt\n");
  #endif
                  dev->last_error = irpts & 0xffff0000;
                  dev->last_interrupt = irpts;
                  return;
              }
  
              // Detect DMA interrupt without transfer complete
              // Currently not supported - all block sizes should fit in the
              //  buffer
              if((irpts & 0x8) && ((irpts & 0x2) != 0x2)) {
  #ifdef EMMC_DEBUG
                  uart_printf("SD: error: DMA interrupt occured without transfer complete\n");
  #endif
                  dev->last_error = irpts & 0xffff0000;
                  dev->last_interrupt = irpts;
                  return;
              }
  
              // Detect transfer complete
              if(irpts & 0x2) {
  #ifdef EMMC_DEBUG
                  uart_printf("SD: SDMA transfer complete");
  #endif
                  // Transfer the data to the user buffer
                  memcpy(dev->buf, (const void *)SDMA_BUFFER, dev->block_size);
              } else {
                  // Unknown error
  #ifdef EMMC_DEBUG
                  if(irpts == 0)
                      uart_printf("SD: timeout waiting for SDMA transfer to complete\n");
                  else
                      uart_printf("SD: unknown SDMA transfer error\n");
  
                  uart_printf("SD: INTERRUPT: %08x, STATUS %08x\n", irpts, mmio_read(EMMC_BASE + EMMC_STATUS));
  #endif
  
                  if((irpts == 0) && ((mmio_read(EMMC_BASE + EMMC_STATUS) & 0x3) == 0x2)) {
                      // The data transfer is ongoing, we should attempt to stop
                      //  it
  #ifdef EMMC_DEBUG
                      uart_printf("SD: aborting transfer\n");
  #endif
                      mmio_write(EMMC_BASE + EMMC_CMDTM, sd_commands[STOP_TRANSMISSION]);
  
  #ifdef EMMC_DEBUG
                      // pause to let us read the screen
                      udelay(2000000);
  #endif
                  }
                  dev->last_error = irpts & 0xffff0000;
                  dev->last_interrupt = irpts;
                  return;
              }
          }
      }
  
      // Return success
      dev->last_cmd_success = 1;
  }
  
  // Handle a card interrupt
  static void sd_handle_card_interrupt(struct emmc_block_dev *dev) {
  #ifdef EMMC_DEBUG
      uint32_t status = mmio_read(EMMC_BASE + EMMC_STATUS);
      uart_printf("SD: card interrupt\n");
      uart_printf("SD: controller status: %08x\n", status);
  #endif
  
      // Get the card status
      if(dev->card_rca) {
          sd_issue_command_int(dev, sd_commands[SEND_STATUS], dev->card_rca << 16,
                               500000);
  #ifdef EMMC_DEBUG
          if(FAIL(dev)) {
              uart_printf("SD: unable to get card status\n");
          } else {
              uart_printf("SD: card status: %08x\n", dev->last_r0);
          }
  #endif
      } else {
  #ifdef EMMC_DEBUG
          uart_printf("SD: no card currently selected\n");
  #endif
      }
  }
  
  static void sd_handle_interrupts(struct emmc_block_dev *dev) {
      uint32_t irpts = mmio_read(EMMC_BASE + EMMC_INTERRUPT);
      uint32_t reset_mask = 0;
  
      if(irpts & SD_COMMAND_COMPLETE) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious command complete interrupt\n");
  #endif
          reset_mask |= SD_COMMAND_COMPLETE;
      }
  
      if(irpts & SD_TRANSFER_COMPLETE) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious transfer complete interrupt\n");
  #endif
          reset_mask |= SD_TRANSFER_COMPLETE;
      }
  
      if(irpts & SD_BLOCK_GAP_EVENT) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious block gap event interrupt\n");
  #endif
          reset_mask |= SD_BLOCK_GAP_EVENT;
      }
  
      if(irpts & SD_DMA_INTERRUPT) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious DMA interrupt\n");
  #endif
          reset_mask |= SD_DMA_INTERRUPT;
      }
  
      if(irpts & SD_BUFFER_WRITE_READY) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious buffer write ready interrupt\n");
  #endif
          reset_mask |= SD_BUFFER_WRITE_READY;
          sd_reset_dat();
      }
  
      if(irpts & SD_BUFFER_READ_READY) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious buffer read ready interrupt\n");
  #endif
          reset_mask |= SD_BUFFER_READ_READY;
          sd_reset_dat();
      }
  
      if(irpts & SD_CARD_INSERTION) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: card insertion detected\n");
  #endif
          reset_mask |= SD_CARD_INSERTION;
      }
  
      if(irpts & SD_CARD_REMOVAL) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: card removal detected\n");
  #endif
          reset_mask |= SD_CARD_REMOVAL;
          dev->card_removal = 1;
      }
  
      if(irpts & SD_CARD_INTERRUPT) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: card interrupt detected\n");
  #endif
          sd_handle_card_interrupt(dev);
          reset_mask |= SD_CARD_INTERRUPT;
      }
  
      if(irpts & 0x8000) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: spurious error interrupt: %08x\n", irpts);
  #endif
          reset_mask |= 0xffff0000;
      }
  
      mmio_write(EMMC_BASE + EMMC_INTERRUPT, reset_mask);
  }
  
  static void sd_issue_command(struct emmc_block_dev *dev, uint32_t command, uint32_t argument, useconds_t timeout) {
      // First, handle any pending interrupts
      sd_handle_interrupts(dev);
  
      // Stop the command issue if it was the card remove interrupt that was
      //  handled
      if(dev->card_removal) {
          dev->last_cmd_success = 0;
          return;
      }
  
      // Now run the appropriate commands by calling sd_issue_command_int()
      if(command & IS_APP_CMD) {
          command &= 0xff;
  #ifdef EMMC_DEBUG
          uart_printf("SD: issuing command ACMD%i\n", command);
  #endif
          if(sd_acommands[command] == SD_CMD_RESERVED(0)) {
              uart_printf("SD: invalid command ACMD%i\n", command);
              dev->last_cmd_success = 0;
              return;
          }
          dev->last_cmd = APP_CMD;
  
          uint32_t rca = 0;
          if(dev->card_rca)
              rca = dev->card_rca << 16;
          sd_issue_command_int(dev, sd_commands[APP_CMD], rca, timeout);
          if(dev->last_cmd_success) {
              dev->last_cmd = command | IS_APP_CMD;
              sd_issue_command_int(dev, sd_acommands[command], argument, timeout);
          }
      } else {
  #ifdef EMMC_DEBUG
          uart_printf("SD: issuing command CMD%i\n", command);
  #endif
  
          if(sd_commands[command] == SD_CMD_RESERVED(0)) {
              uart_printf("SD: invalid command CMD%i\n", command);
              dev->last_cmd_success = 0;
              return;
          }
  
          dev->last_cmd = command;
          sd_issue_command_int(dev, sd_commands[command], argument, timeout);
      }
  
  #ifdef EMMC_DEBUG
      if(FAIL(dev)) {
          uart_printf("SD: error issuing command: interrupts %08x: ", dev->last_interrupt);
          if(dev->last_error == 0)
              uart_printf("TIMEOUT");
          else {
              for(int i = 0; i < SD_ERR_RSVD; i++) {
                  if(dev->last_error & (1 << (i + 16))) {
                      uart_printf(err_irpts[i]);
                      uart_printf(" ");
                  }
              }
          }
  		uart_printf("\n");
      } else
          uart_printf("SD: command completed successfully\n");
  #endif
  }
  
  int sd_card_init(struct block_device **dev) {
      // Check the sanity of the sd_commands and sd_acommands structures
      if(sizeof(sd_commands) != (64 * sizeof(uint32_t))) {
          uart_printf("EMMC: fatal error, sd_commands of incorrect size: %i"
                 " expected %i\n", sizeof(sd_commands),
                 64 * sizeof(uint32_t));
          return 1;
      }
      if(sizeof(sd_acommands) != (64 * sizeof(uint32_t))) {
          uart_printf("EMMC: fatal error, sd_acommands of incorrect size: %i"
                 " expected %i\n", sizeof(sd_acommands),
                 64 * sizeof(uint32_t));
          return 1;
      }
  
      // Power cycle the card to ensure its in its startup state
      if(bcm_2708_power_cycle() != 0) {
          uart_printf("EMMC: BCM2708 controller did not power cycle successfully\n");
      }
  #ifdef EMMC_DEBUG
      else
  		uart_printf("EMMC: BCM2708 controller power-cycled\n");
  #endif
  
      // Read the controller version
      uint32_t ver = mmio_read(EMMC_BASE + EMMC_SLOTISR_VER);
      uint32_t vendor = ver >> 24;
      uint32_t sdversion = (ver >> 16) & 0xff;
      uint32_t slot_status = ver & 0xff;
      uart_printf("EMMC: vendor %x, sdversion %x, slot_status %x\n", vendor, sdversion, slot_status);
      hci_ver = sdversion;
  
      if(hci_ver < 2) {
          uart_printf("EMMC: only SDHCI versions >= 3.0 are supported\n");
  		return 1;
      }
  
      // Reset the controller
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: resetting controller\n");
  #endif
      uint32_t control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
      control1 |= (1 << 24);
      // Disable clock
      control1 &= ~(1 << 2);
      control1 &= ~(1 << 0);
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      TIMEOUT_WAIT((mmio_read(EMMC_BASE + EMMC_CONTROL1) & (0x7 << 24)) == 0, 1000000);
      if((mmio_read(EMMC_BASE + EMMC_CONTROL1) & (0x7 << 24)) != 0)
      {
          uart_printf("EMMC: controller did not reset properly\n");
          return 2;
      }
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: control0: %08x, control1: %08x, control2: %08x\n",
  			mmio_read(EMMC_BASE + EMMC_CONTROL0),
  			mmio_read(EMMC_BASE + EMMC_CONTROL1),
              mmio_read(EMMC_BASE + EMMC_CONTROL2));
  #endif
  
      // Read the capabilities registers
      capabilities_0 = mmio_read(EMMC_BASE + EMMC_CAPABILITIES_0);
      capabilities_1 = mmio_read(EMMC_BASE + EMMC_CAPABILITIES_1);
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: capabilities: %08x%08x\n", capabilities_1, capabilities_0);
  #endif
  
      // Check for a valid card
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: checking for an inserted card\n");
  #endif
      TIMEOUT_WAIT(mmio_read(EMMC_BASE + EMMC_STATUS) & (1 << 16), 500000);
      uint32_t status_reg = mmio_read(EMMC_BASE + EMMC_STATUS);
      if((status_reg & (1 << 16)) == 0) {
          uart_printf("EMMC: no card inserted\n");
          return 2;
      }
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: status: %08x\n", status_reg);
  #endif
  
      // Clear control2
      mmio_write(EMMC_BASE + EMMC_CONTROL2, 0);
  
      // Get the base clock rate
      uint32_t base_clock = sd_get_base_clock_hz();
      if(base_clock == 0) {
          uart_printf("EMMC: assuming clock rate to be 100MHz\n");
          base_clock = 100000000;
      }
  
      // Set clock rate to something slow
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: setting clock rate\n");
  #endif
      control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
      control1 |= 1;			// enable clock
  
      // Set to identification frequency (400 kHz)
      uint32_t f_id = sd_get_clock_divider(base_clock, SD_CLOCK_ID);
      if(f_id == SD_GET_CLOCK_DIVIDER_FAIL) {
          uart_printf("EMMC: unable to get a valid clock divider for ID frequency\n");
          return 3;
      }
      control1 |= f_id;
  
      control1 |= (7 << 16);		// data timeout = TMCLK * 2^10
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      TIMEOUT_WAIT(mmio_read(EMMC_BASE + EMMC_CONTROL1) & 0x2, 0x1000000);
      if((mmio_read(EMMC_BASE + EMMC_CONTROL1) & 0x2) == 0) {
          uart_printf("EMMC: controller's clock did not stabilise within 1 second\n");
          return 2;
      }
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: control0: %08x, control1: %08x\n",
  			mmio_read(EMMC_BASE + EMMC_CONTROL0),
  			mmio_read(EMMC_BASE + EMMC_CONTROL1));
  #endif
  
      // Enable the SD clock
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: enabling SD clock\n");
  #endif
      udelay(2000);
      control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
      control1 |= 4;
      mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
      udelay(2000);
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: SD clock enabled\n");
  #endif
  
      // Mask off sending interrupts to the ARM
      mmio_write(EMMC_BASE + EMMC_IRPT_EN, 0);
      // Reset interrupts
      mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffffffff);
      // Have all interrupts sent to the INTERRUPT register
      mmio_write(EMMC_BASE + EMMC_IRPT_MASK, 0xffffffff);
  
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: interrupts disabled\n");
  #endif
      udelay(2000);
  
      // Prepare the device structure
      struct emmc_block_dev *ret;
      if(*dev == 0x0)
          ret = (struct emmc_block_dev *)kmalloc(sizeof(struct emmc_block_dev));
      else
          ret = (struct emmc_block_dev *)*dev;
  
      if(ret == 0x0) {
          uart_printf("EMMC: Failed preparing device\n");
          return 2;
      }
  
      memset(ret, 0, sizeof(struct emmc_block_dev));
      ret->bd.driver_name = driver_name;
      ret->bd.device_name = device_name;
      ret->bd.block_size = 512;
      ret->bd.read = sd_read;
      ret->bd.write = sd_write;
      ret->bd.supports_multiple_block_read = 1;
      ret->bd.supports_multiple_block_write = 1;
      ret->base_clock = base_clock;
  
  #ifdef EMMC_DEBUG
      uart_printf("EMMC: device structure created\n");
  #endif
  
      // Send CMD0 to the card (reset to idle state)
      sd_issue_command(ret, GO_IDLE_STATE, 0, 500000);
      if(FAIL(ret)) {
          uart_printf("SD: no CMD0 response\n");
          return 2;
      }
  
      // Send CMD8 to the card
      // Voltage supplied = 0x1 = 2.7-3.6V (standard)
      // Check pattern = 10101010b (as per PLSS 4.3.13) = 0xAA
  #ifdef EMMC_DEBUG
      uart_printf("SD: note a timeout error on the following command (CMD8) is normal "
             "and expected if the SD card version is less than 2.0\n");
  #endif
      sd_issue_command(ret, SEND_IF_COND, 0x1aa, 500000);
      int v2_later = 0;
      if(TIMEOUT(ret))
          v2_later = 0;
      else if(CMD_TIMEOUT(ret)) {
          if(sd_reset_cmd() == -1)
              return 2;
          mmio_write(EMMC_BASE + EMMC_INTERRUPT, SD_ERR_MASK_CMD_TIMEOUT);
          v2_later = 0;
      } else if(FAIL(ret)) {
          uart_printf("SD: failure sending CMD8 (%08x)\n", ret->last_interrupt);
          return 3;
      } else {
          if((ret->last_r0 & 0xfff) != 0x1aa) {
              uart_printf("SD: unusable card\n");
  #ifdef EMMC_DEBUG
              uart_printf("SD: CMD8 response %08x\n", ret->last_r0);
  #endif
              return 1;
          } else
              v2_later = 1;
      }
  
      // Here we are supposed to check the response to CMD5 (HCSS 3.6)
      // It only returns if the card is a SDIO card
  #ifdef EMMC_DEBUG
      uart_printf("SD: note that a timeout error on the following command (CMD5) is "
             "normal and expected if the card is not a SDIO card.\n");
  #endif
      sd_issue_command(ret, IO_SET_OP_COND, 0, 10000);
      if(!TIMEOUT(ret)) {
          if(CMD_TIMEOUT(ret)) {
              if(sd_reset_cmd() == -1)
                  return 2;
              mmio_write(EMMC_BASE + EMMC_INTERRUPT, SD_ERR_MASK_CMD_TIMEOUT);
          } else {
              uart_printf("SD: SDIO card detected - not currently supported\n");
  #ifdef EMMC_DEBUG
              uart_printf("SD: CMD5 returned %08x\n", ret->last_r0);
  #endif
              return 3;
          }
      }
  
      // Call an inquiry ACMD41 (voltage window = 0) to get the OCR
  #ifdef EMMC_DEBUG
      uart_printf("SD: sending inquiry ACMD41\n");
  #endif
      sd_issue_command(ret, ACMD(41), 0, 500000);
      if(FAIL(ret)) {
          uart_printf("SD: inquiry ACMD41 failed\n");
          return 2;
      }
  #ifdef EMMC_DEBUG
      uart_printf("SD: inquiry ACMD41 returned %08x\n", ret->last_r0);
  #endif
  
      // Call initialization ACMD41
      int card_is_busy = 1;
      while(card_is_busy) {
          uint32_t v2_flags = 0;
          if(v2_later) {
              // Set SDHC support
              v2_flags |= (1 << 30);
              // Set 1.8v support
              if(!ret->failed_voltage_switch)
                  v2_flags |= (1 << 24);
          }
  
          sd_issue_command(ret, ACMD(41), 0x00ff8000 | v2_flags, 500000);
          if(FAIL(ret)) {
              uart_printf("SD: error issuing ACMD41\n");
              return 2;
          }
  
          if((ret->last_r0 >> 31) & 0x1) {
              // Initialization is complete
              ret->card_ocr = (ret->last_r0 >> 8) & 0xffff;
              ret->card_supports_sdhc = (ret->last_r0 >> 30) & 0x1;
  
              if(!ret->failed_voltage_switch)
                  ret->card_supports_18v = (ret->last_r0 >> 24) & 0x1;
  
              card_is_busy = 0;
          } else {
              // Card is still busy
  #ifdef EMMC_DEBUG
              uart_printf("SD: card is busy, retrying\n");
  #endif
              udelay(500000);
          }
      }
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: card identified: OCR: %04x, 1.8v support: %d, SDHC support: %d\n",
  			ret->card_ocr, ret->card_supports_18v, ret->card_supports_sdhc);
  #endif
  
      // At this point, we know the card is definitely an SD card, so will definitely
      //  support SDR12 mode which runs at 25 MHz
      sd_switch_clock_rate(base_clock, SD_CLOCK_NORMAL);
  
      // A small wait before the voltage switch
      udelay(5000);
  
      // Switch to 1.8V mode if possible
      if(ret->card_supports_18v) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: switching to 1.8V mode\n");
  #endif
          // As per HCSS 3.6.1
  
          // Send VOLTAGE_SWITCH
          sd_issue_command(ret, VOLTAGE_SWITCH, 0, 500000);
          if(FAIL(ret)) {
  #ifdef EMMC_DEBUG
              uart_printf("SD: error issuing VOLTAGE_SWITCH\n");
  #endif
              ret->failed_voltage_switch = 1;
              sd_power_off();
              return sd_card_init((struct block_device **)&ret);
          }
  
          // Disable SD clock
          control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
          control1 &= ~(1 << 2);
          mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
  
          // Check DAT[3:0]
          status_reg = mmio_read(EMMC_BASE + EMMC_STATUS);
          uint32_t dat30 = (status_reg >> 20) & 0xf;
          if(dat30 != 0) {
  #ifdef EMMC_DEBUG
              uart_printf("SD: DAT[3:0] did not settle to 0\n");
  #endif
              ret->failed_voltage_switch = 1;
              sd_power_off();
              return sd_card_init((struct block_device **)&ret);
          }
  
          // Set 1.8V signal enable to 1
          uint32_t control0 = mmio_read(EMMC_BASE + EMMC_CONTROL0);
          control0 |= (1 << 8);
          mmio_write(EMMC_BASE + EMMC_CONTROL0, control0);
  
          // Wait 5 ms
          udelay(5000);
  
          // Check the 1.8V signal enable is set
          control0 = mmio_read(EMMC_BASE + EMMC_CONTROL0);
          if(((control0 >> 8) & 0x1) == 0) {
  #ifdef EMMC_DEBUG
              uart_printf("SD: controller did not keep 1.8V signal enable high\n");
  #endif
              ret->failed_voltage_switch = 1;
              sd_power_off();
              return sd_card_init((struct block_device **)&ret);
          }
  
          // Re-enable the SD clock
          control1 = mmio_read(EMMC_BASE + EMMC_CONTROL1);
          control1 |= (1 << 2);
          mmio_write(EMMC_BASE + EMMC_CONTROL1, control1);
  
          // Wait 1 ms
          udelay(10000);
  
          // Check DAT[3:0]
          status_reg = mmio_read(EMMC_BASE + EMMC_STATUS);
          dat30 = (status_reg >> 20) & 0xf;
          if(dat30 != 0xf) {
  #ifdef EMMC_DEBUG
              uart_printf("SD: DAT[3:0] did not settle to 1111b (%01x)\n", dat30);
  #endif
              ret->failed_voltage_switch = 1;
              sd_power_off();
              return sd_card_init((struct block_device **)&ret);
          }
  #ifdef EMMC_DEBUG
          uart_printf("SD: voltage switch complete\n");
  #endif
      }
  
      // Send CMD2 to get the cards CID
      sd_issue_command(ret, ALL_SEND_CID, 0, 500000);
      if(FAIL(ret)) {
          uart_printf("SD: error sending ALL_SEND_CID\n");
          return 2;
      }
      uint32_t card_cid_0 = ret->last_r0;
      uint32_t card_cid_1 = ret->last_r1;
      uint32_t card_cid_2 = ret->last_r2;
      uint32_t card_cid_3 = ret->last_r3;
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: card CID: %08x%08x%08x%08x\n", card_cid_3, card_cid_2, card_cid_1, card_cid_0);
  #endif
      uint32_t *dev_id = (uint32_t *)kmalloc(4 * sizeof(uint32_t));
      dev_id[0] = card_cid_0;
      dev_id[1] = card_cid_1;
      dev_id[2] = card_cid_2;
      dev_id[3] = card_cid_3;
      ret->bd.device_id = (uint8_t *)dev_id;
      ret->bd.dev_id_len = 4 * sizeof(uint32_t);
  
      // Send CMD3 to enter the data state
      sd_issue_command(ret, SEND_RELATIVE_ADDR, 0, 500000);
      if(FAIL(ret)) {
          uart_printf("SD: error sending SEND_RELATIVE_ADDR\n");
          kfree(ret);
          return -1;
      }
  
      uint32_t cmd3_resp = ret->last_r0;
  #ifdef EMMC_DEBUG
      uart_printf("SD: CMD3 response: %08x\n", cmd3_resp);
  #endif
  
      ret->card_rca = (cmd3_resp >> 16) & 0xffff;
      uint32_t crc_error = (cmd3_resp >> 15) & 0x1;
      uint32_t illegal_cmd = (cmd3_resp >> 14) & 0x1;
      uint32_t error = (cmd3_resp >> 13) & 0x1;
      uint32_t status = (cmd3_resp >> 9) & 0xf;
      uint32_t ready = (cmd3_resp >> 8) & 0x1;
  
      if(crc_error) {
          uart_printf("SD: CRC error\n");
          kfree(ret);
          kfree(dev_id);
          return 3;
      }
  
      if(illegal_cmd) {
          uart_printf("SD: illegal command\n");
          kfree(ret);
          kfree(dev_id);
          return 3;
      }
  
      if(error) {
          uart_printf("SD: generic error\n");
          kfree(ret);
          kfree(dev_id);
          return 1;
      }
  
      if(!ready) {
          uart_printf("SD: not ready for data\n");
          kfree(ret);
          kfree(dev_id);
          return 2;
      }
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: RCA: %04x\n", ret->card_rca);
  #endif
      // Now select the card (toggles it to transfer state)
      sd_issue_command(ret, SELECT_CARD, ret->card_rca << 16, 500000);
      if(FAIL(ret)) {
          uart_printf("SD: error sending CMD7\n");
          kfree(ret);
          return 2;
      }
  
      uint32_t cmd7_resp = ret->last_r0;
      status = (cmd7_resp >> 9) & 0xf;
  
      if((status != 3) && (status != 4)) {
          uart_printf("SD: invalid status (%d)\n", status);
          kfree(ret);
          kfree(dev_id);
          return 1;
      }
  
      // If not an SDHC card, ensure BLOCKLEN is 512 bytes
      if(!ret->card_supports_sdhc) {
          sd_issue_command(ret, SET_BLOCKLEN, 512, 500000);
          if(FAIL(ret)) {
              uart_printf("SD: error sending SET_BLOCKLEN\n");
              kfree(ret);
              return 2;
          }
      }
      ret->block_size = 512;
      uint32_t controller_block_size = mmio_read(EMMC_BASE + EMMC_BLKSIZECNT);
      controller_block_size &= (~0xfff);
      controller_block_size |= 0x200;
      mmio_write(EMMC_BASE + EMMC_BLKSIZECNT, controller_block_size);
  
      // Get the cards SCR register
      ret->scr = (struct sd_scr *)kmalloc(sizeof(struct sd_scr));
      ret->buf = &ret->scr->scr[0];
      ret->block_size = 8;
      ret->blocks_to_transfer = 1;
      sd_issue_command(ret, SEND_SCR, 0, 500000);
      ret->block_size = 512;
      if(FAIL(ret)) {
          uart_printf("SD: error sending SEND_SCR\n");
          kfree(ret->scr);
          kfree(ret);
          return 2;
      }
  
      // Determine card version
      // Note that the SCR is big-endian
      uint32_t scr0 = byte_swap(ret->scr->scr[0]);
      ret->scr->sd_version = SD_VER_UNKNOWN;
      uint32_t sd_spec = (scr0 >> (56 - 32)) & 0xf;
      uint32_t sd_spec3 = (scr0 >> (47 - 32)) & 0x1;
      uint32_t sd_spec4 = (scr0 >> (42 - 32)) & 0x1;
      ret->scr->sd_bus_widths = (scr0 >> (48 - 32)) & 0xf;
      if(sd_spec == 0)
          ret->scr->sd_version = SD_VER_1;
      else if(sd_spec == 1)
          ret->scr->sd_version = SD_VER_1_1;
      else if(sd_spec == 2) {
          if(sd_spec3 == 0)
              ret->scr->sd_version = SD_VER_2;
          else if(sd_spec3 == 1) {
              if(sd_spec4 == 0)
                  ret->scr->sd_version = SD_VER_3;
              else if(sd_spec4 == 1)
                  ret->scr->sd_version = SD_VER_4;
          }
      }
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: &scr: %08x\n", &ret->scr->scr[0]);
      uart_printf("SD: SCR[0]: %08x, SCR[1]: %08x\n", ret->scr->scr[0], ret->scr->scr[1]);;
      uart_printf("SD: SCR: %08x%08x\n", byte_swap(ret->scr->scr[0]), byte_swap(ret->scr->scr[1]));
      uart_printf("SD: SCR: version %s, bus_widths %01x\n", sd_versions[ret->scr->sd_version],
             ret->scr->sd_bus_widths);
  #endif
  
      if(ret->scr->sd_bus_widths & 0x4) {
          // Set 4-bit transfer mode (ACMD6)
          // See HCSS 3.4 for the algorithm
  #ifdef EMMC_DEBUG
          uart_printf("SD: switching to 4-bit data mode\n");
  #endif
  
          // Disable card interrupt in host
          uint32_t old_irpt_mask = mmio_read(EMMC_BASE + EMMC_IRPT_MASK);
          uint32_t new_iprt_mask = old_irpt_mask & ~(1 << 8);
          mmio_write(EMMC_BASE + EMMC_IRPT_MASK, new_iprt_mask);
  
          // Send ACMD6 to change the card's bit mode
          sd_issue_command(ret, SET_BUS_WIDTH, 0x2, 500000);
          if(FAIL(ret))
              uart_printf("SD: switch to 4-bit data mode failed\n");
          else {
              // Change bit mode for Host
              uint32_t control0 = mmio_read(EMMC_BASE + EMMC_CONTROL0);
              control0 |= 0x2;
              mmio_write(EMMC_BASE + EMMC_CONTROL0, control0);
  
              // Re-enable card interrupt in host
              mmio_write(EMMC_BASE + EMMC_IRPT_MASK, old_irpt_mask);
  
  #ifdef EMMC_DEBUG
              uart_printf("SD: switch to 4-bit complete\n");
  #endif
          }
      }
  
      uart_printf("SD: found a valid version %s SD card\n", sd_versions[ret->scr->sd_version]);
  #ifdef EMMC_DEBUG
      uart_printf("SD: setup successful (status %d)\n", status);
  #endif
  
      // Reset interrupt register
      mmio_write(EMMC_BASE + EMMC_INTERRUPT, 0xffffffff);
  
      *dev = (struct block_device *)ret;
  
      return 0;
  }
  
  static int sd_ensure_data_mode(struct emmc_block_dev *edev)
  {
      if(edev->card_rca == 0) {
          // Try again to initialise the card
          int ret = sd_card_init((struct block_device **)&edev);
          if(ret != 0)
              return ret;
      }
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: ensure_data_mode() obtaining status register for card_rca %08x: ",
  		edev->card_rca);
  #endif
  
      sd_issue_command(edev, SEND_STATUS, edev->card_rca << 16, 500000);
      if(FAIL(edev)) {
          uart_printf("SD: ensure_data_mode() error sending CMD13\n");
          edev->card_rca = 0;
          return -1;
      }
  
      uint32_t status = edev->last_r0;
      uint32_t cur_state = (status >> 9) & 0xf;
  #ifdef EMMC_DEBUG
      uart_printf("status %d\n", cur_state);
  #endif
      if(cur_state == 3) {
          // Currently in the stand-by state - select it
          sd_issue_command(edev, SELECT_CARD, edev->card_rca << 16, 500000);
          if(FAIL(edev)) {
              uart_printf("SD: ensure_data_mode() no response from CMD17\n");
              edev->card_rca = 0;
              return -1;
          }
      } else if(cur_state == 5) {
          // In the data transfer state - cancel the transmission
          sd_issue_command(edev, STOP_TRANSMISSION, 0, 500000);
          if(FAIL(edev)) {
              uart_printf("SD: ensure_data_mode() no response from CMD12\n");
              edev->card_rca = 0;
              return -1;
          }
  
          // Reset the data circuit
          sd_reset_dat();
      } else if(cur_state != 4) {
          // Not in the transfer state - re-initialise
          int ret = sd_card_init((struct block_device **)&edev);
          if(ret != 0)
              return ret;
      }
  
      // Check again that we're now in the correct mode
      if(cur_state != 4) {
  #ifdef EMMC_DEBUG
          uart_printf("SD: ensure_data_mode() rechecking status: ");
  #endif
          sd_issue_command(edev, SEND_STATUS, edev->card_rca << 16, 500000);
          if(FAIL(edev)) {
              uart_printf("SD: ensure_data_mode() no response from CMD13\n");
              edev->card_rca = 0;
              return -1;
          }
          status = edev->last_r0;
          cur_state = (status >> 9) & 0xf;
  
  #ifdef EMMC_DEBUG
          uart_printf("%d\n", cur_state);
  #endif
  
          if(cur_state != 4) {
              uart_printf("SD: unable to initialise SD card to "
                     "data mode (state %d)\n", cur_state);
              edev->card_rca = 0;
              return -1;
          }
      }
  
      return 0;
  }
  
  static int sd_do_data_command(struct emmc_block_dev *edev, int is_write, uint8_t *buf, uint64_t buf_size, uint32_t block_no) {
      // PLSS table 4.20 - SDSC cards use byte addresses rather than block addresses
      if(!edev->card_supports_sdhc)
          block_no *= 512;
  
      // This is as per HCSS 3.7.2.1
      if(buf_size < edev->block_size) {
          uart_printf("SD: do_data_command() called with buffer size (%d) less than "
                 "block size (%d)\n", buf_size, edev->block_size);
          return -1;
      }
  
      divmod_t mod = divmod(buf_size, edev->block_size);
      edev->blocks_to_transfer = mod.div;
      if(mod.mod) {
          uart_printf("SD: do_data_command() called with buffer size (%d) not an "
                 "exact multiple of block size (%d)\n", buf_size, edev->block_size);
          return -1;
      }
      edev->buf = buf;
  
      // Decide on the command to use
      int command;
      if(is_write) {
          if(edev->blocks_to_transfer > 1)
              command = WRITE_MULTIPLE_BLOCK;
          else
              command = WRITE_BLOCK;
      } else {
          if(edev->blocks_to_transfer > 1)
              command = READ_MULTIPLE_BLOCK;
          else
              command = READ_SINGLE_BLOCK;
      }
  
      int retry_count = 0;
      int max_retries = 3;
      while(retry_count < max_retries) {
          edev->use_sdma = 0;
          sd_issue_command(edev, command, block_no, 5000000);
  
          if(SUCCESS(edev))
              break;
          else {
              uart_printf("SD: error sending CMD%d, ", command);
              uart_printf("error = %08x.  ", edev->last_error);
              retry_count++;
              if(retry_count < max_retries)
                  uart_printf("Retrying...\n");
              else
                  uart_printf("Giving up.\n");
          }
      }
      if(retry_count >= max_retries) {
          edev->card_rca = 0;
          return -1;
      }
  
      return 0;
  }
  int sd_read(struct block_device *dev, uint8_t *buf, uint64_t buf_size, uint32_t block_no) {
      // Check the status of the card
      struct emmc_block_dev *edev = (struct emmc_block_dev *)dev;
      if(sd_ensure_data_mode(edev) != 0)
          return 2;
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: read() card ready, reading from block %u\n", block_no);
  #endif
  
      if(sd_do_data_command(edev, 0, buf, buf_size, block_no) < 0)
          return 1;
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: data read successful\n");
  #endif
  
      return 0;
  }
  
  int sd_write(struct block_device *dev, uint8_t *buf, uint64_t buf_size, uint32_t block_no) {
      // Check the status of the card
      struct emmc_block_dev *edev = (struct emmc_block_dev *)dev;
      if(sd_ensure_data_mode(edev) != 0)
          return -1;
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: write() card ready, reading from block %u\n", block_no);
  #endif
  
      if(sd_do_data_command(edev, 1, buf, buf_size, block_no) < 0)
          return -1;
  
  #ifdef EMMC_DEBUG
      uart_printf("SD: write read successful\n");
  #endif
  
      return 0;
  }
  
  