+static int
+sdcard_set_power(struct sdhci_s *regs)
+{
+ u32 cap = readl(®s->cap_lo);
+ u32 volt, vbits;
+ if (cap & SD_CAPLO_V33) {
+ volt = 1<<20;
+ vbits = SPC_V33;
+ } else if (cap & SD_CAPLO_V30) {
+ volt = 1<<18;
+ vbits = SPC_V30;
+ } else if (cap & SD_CAPLO_V18) {
+ volt = 1<<7;
+ vbits = SPC_V18;
+ } else {
+ dprintf(1, "SD controller unsupported volt range (%x)\n", cap);
+ return -1;
+ }
+ writeb(®s->power_control, 0);
+ msleep(SDHCI_POWER_OFF_TIME);
+ writeb(®s->power_control, vbits | SPC_POWER_ON);
+ msleep(SDHCI_POWER_ON_TIME);
+ return volt;
+}
+
+static int
+sdcard_set_frequency(struct sdhci_s *regs, u32 khz)
+{
+ u16 ver = readw(®s->controller_version);
+ u32 cap = readl(®s->cap_lo);
+ u32 base_freq = (cap >> SD_CAPLO_BASECLOCK_SHIFT) & SD_CAPLO_BASECLOCK_MASK;
+ if (!base_freq) {
+ dprintf(1, "Unknown base frequency for SD controller\n");
+ return -1;
+ }
+ // Set new frequency
+ u32 divisor = DIV_ROUND_UP(base_freq * 1000, khz);
+ u16 creg;
+ if ((ver & 0xff) <= 0x01) {
+ divisor = divisor > 1 ? 1 << __fls(divisor-1) : 0;
+ creg = (divisor & SCC_SDCLK_MASK) << SCC_SDCLK_SHIFT;
+ } else {
+ divisor = DIV_ROUND_UP(divisor, 2);
+ creg = (divisor & SCC_SDCLK_MASK) << SCC_SDCLK_SHIFT;
+ creg |= (divisor & SCC_SDCLK_HI_MASK) >> SCC_SDCLK_HI_RSHIFT;
+ }
+ dprintf(3, "sdcard_set_frequency %d %d %x\n", base_freq, khz, creg);
+ writew(®s->clock_control, 0);
+ writew(®s->clock_control, creg | SCC_INTERNAL_ENABLE);
+ // Wait for frequency to become active
+ int ret = sdcard_waitw(®s->clock_control, SCC_STABLE);
+ if (ret < 0)
+ return ret;
+ // Enable SD clock
+ writew(®s->clock_control, creg | SCC_INTERNAL_ENABLE | SCC_CLOCK_ENABLE);
+ return 0;
+}
+
+// Obtain the disk size of an SD card
+static int
+sdcard_get_capacity(struct sddrive_s *drive, u8 *csd)
+{
+ // Original MMC/SD card capacity formula
+ u16 C_SIZE = (csd[6] >> 6) | (csd[7] << 2) | ((csd[8] & 0x03) << 10);
+ u8 C_SIZE_MULT = (csd[4] >> 7) | ((csd[5] & 0x03) << 1);
+ u8 READ_BL_LEN = csd[9] & 0x0f;
+ u32 count = (C_SIZE+1) << (C_SIZE_MULT + 2 + READ_BL_LEN - 9);
+ // Check for newer encoding formats.
+ u8 CSD_STRUCTURE = csd[14] >> 6;
+ if ((drive->card_type & SF_MMC) && CSD_STRUCTURE >= 2) {
+ // Get capacity from EXT_CSD register
+ u8 ext_csd[512];
+ int ret = sdcard_pio_transfer(drive, SC_SEND_EXT_CSD, 0, ext_csd, 1);
+ if (ret)
+ return ret;
+ count = *(u32*)&ext_csd[212];
+ } else if (!(drive->card_type & SF_MMC) && CSD_STRUCTURE >= 1) {
+ // High capacity SD card
+ u32 C_SIZE2 = csd[5] | (csd[6] << 8) | ((csd[7] & 0x3f) << 16);
+ count = (C_SIZE2+1) << (19-9);
+ }
+ // Fill drive struct and return
+ drive->drive.blksize = DISK_SECTOR_SIZE;
+ drive->drive.sectors = count;
+ return 0;
+}
+