diff options
-rw-r--r-- | drivers/mci/mci-core.c | 22 |
1 files changed, 15 insertions, 7 deletions
diff --git a/drivers/mci/mci-core.c b/drivers/mci/mci-core.c index 0d601e3385..88892b646f 100644 --- a/drivers/mci/mci-core.c +++ b/drivers/mci/mci-core.c @@ -758,15 +758,22 @@ static void mci_extract_card_capacity_from_csd(struct mci *mci) uint64_t csize, cmult; if (mci->high_capacity) { - csize = (mci->csd[1] & 0x3f) << 16 | (mci->csd[2] & 0xffff0000) >> 16; - cmult = 8; + if (IS_SD(mci)) { + csize = UNSTUFF_BITS(mci->csd, 48, 22); + mci->capacity = (1 + csize) << 10; + } else { + mci->capacity = mci->ext_csd[EXT_CSD_SEC_CNT] << 0 | + mci->ext_csd[EXT_CSD_SEC_CNT + 1] << 8 | + mci->ext_csd[EXT_CSD_SEC_CNT + 2] << 16 | + mci->ext_csd[EXT_CSD_SEC_CNT + 3] << 24; + } } else { - csize = (mci->csd[1] & 0x3ff) << 2 | (mci->csd[2] & 0xc0000000) >> 30; - cmult = (mci->csd[2] & 0x00038000) >> 15; + cmult = UNSTUFF_BITS(mci->csd, 47, 3); + csize = UNSTUFF_BITS(mci->csd, 62, 12); + mci->capacity = (csize + 1) << (cmult + 2); } - mci->capacity = (csize + 1) << (cmult + 2); - mci->capacity *= mci->read_bl_len; + mci->capacity *= 1 << UNSTUFF_BITS(mci->csd, 80, 4);; dev_dbg(mci->mci_dev, "Capacity: %u MiB\n", (unsigned)(mci->capacity >> 20)); } @@ -996,7 +1003,6 @@ static int mci_startup(struct mci *mci) mci_detect_version_from_csd(mci); mci_extract_max_tran_speed_from_csd(mci); mci_extract_block_lengths_from_csd(mci); - mci_extract_card_capacity_from_csd(mci); /* sanitiy? */ if (mci->read_bl_len > SECTOR_SIZE) { @@ -1032,6 +1038,8 @@ static int mci_startup(struct mci *mci) if (err) return err; + mci_extract_card_capacity_from_csd(mci); + /* Restrict card's capabilities by what the host can do */ mci->card_caps &= host->host_caps; |