2 files changed, 91 insertions, 4 deletions

diff --git a/drivers/mmc/core/core.c b/drivers/mmc/core/core.c
index f7284b905eb3..df6ce4a06cf3 100644
--- a/drivers/mmc/core/core.c
+++ b/drivers/mmc/core/core.c
@@ -20,6 +20,7 @@
 #include <linux/err.h>
 #include <linux/leds.h>
 #include <linux/scatterlist.h>
+#include <linux/log2.h>
 
 #include <linux/mmc/card.h>
 #include <linux/mmc/host.h>
@@ -448,6 +449,80 @@ void mmc_set_bus_width(struct mmc_host *host, unsigned int width)
         mmc_set_ios(host);
 }
 
+/**
+ * mmc_vdd_to_ocrbitnum - Convert a voltage to the OCR bit number
+ * @vdd:        voltage (mV)
+ * @low_bits:   prefer low bits in boundary cases
+ *
+ * This function returns the OCR bit number according to the provided @vdd
+ * value. If conversion is not possible a negative errno value returned.
+ *
+ * Depending on the @low_bits flag the function prefers low or high OCR bits
+ * on boundary voltages. For example,
+ * with @low_bits = true, 3300 mV translates to ilog2(MMC_VDD_32_33);
+ * with @low_bits = false, 3300 mV translates to ilog2(MMC_VDD_33_34);
+ *
+ * Any value in the [1951:1999] range translates to the ilog2(MMC_VDD_20_21).
+ */
+static int mmc_vdd_to_ocrbitnum(int vdd, bool low_bits)
+{
+        const int max_bit = ilog2(MMC_VDD_35_36);
+        int bit;
+
+        if (vdd < 1650 || vdd > 3600)
+                return -EINVAL;
+
+        if (vdd >= 1650 && vdd <= 1950)
+                return ilog2(MMC_VDD_165_195);
+
+        if (low_bits)
+                vdd -= 1;
+
+        /* Base 2000 mV, step 100 mV, bit's base 8. */
+        bit = (vdd - 2000) / 100 + 8;
+        if (bit > max_bit)
+                return max_bit;
+        return bit;
+}
+
+/**
+ * mmc_vddrange_to_ocrmask - Convert a voltage range to the OCR mask
+ * @vdd_min:    minimum voltage value (mV)
+ * @vdd_max:    maximum voltage value (mV)
+ *
+ * This function returns the OCR mask bits according to the provided @vdd_min
+ * and @vdd_max values. If conversion is not possible the function returns 0.
+ *
+ * Notes wrt boundary cases:
+ * This function sets the OCR bits for all boundary voltages, for example
+ * [3300:3400] range is translated to MMC_VDD_32_33 | MMC_VDD_33_34 |
+ * MMC_VDD_34_35 mask.
+ */
+u32 mmc_vddrange_to_ocrmask(int vdd_min, int vdd_max)
+{
+        u32 mask = 0;
+
+        if (vdd_max < vdd_min)
+                return 0;
+
+        /* Prefer high bits for the boundary vdd_max values. */
+        vdd_max = mmc_vdd_to_ocrbitnum(vdd_max, false);
+        if (vdd_max < 0)
+                return 0;
+
+        /* Prefer low bits for the boundary vdd_min values. */
+        vdd_min = mmc_vdd_to_ocrbitnum(vdd_min, true);
+        if (vdd_min < 0)
+                return 0;
+
+        /* Fill the mask, from max bit to min bit. */
+        while (vdd_max >= vdd_min)
+                mask |= 1 << vdd_max--;
+
+        return mask;
+}
+EXPORT_SYMBOL(mmc_vddrange_to_ocrmask);
+
 /*
  * Mask off any voltages we don't support and select
  * the lowest voltage
@@ -467,6 +542,8 @@ u32 mmc_select_voltage(struct mmc_host *host, u32 ocr)
                 host->ios.vdd = bit;
                 mmc_set_ios(host);
         } else {
+                pr_warning("%s: host doesn't support card's voltages\n",
+                                mmc_hostname(host));
                 ocr = 0;
         }
 
diff --git a/drivers/mmc/core/mmc.c b/drivers/mmc/core/mmc.c
index fdd7c760be8c..c232d11a7ed4 100644
--- a/drivers/mmc/core/mmc.c
+++ b/drivers/mmc/core/mmc.c
@@ -434,13 +434,24 @@ static int mmc_init_card(struct mmc_host *host, u32 ocr,
          * Activate wide bus (if supported).
          */
         if ((card->csd.mmca_vsn >= CSD_SPEC_VER_4) &&
-                (host->caps & MMC_CAP_4_BIT_DATA)) {
+            (host->caps & (MMC_CAP_4_BIT_DATA | MMC_CAP_8_BIT_DATA))) {
+                unsigned ext_csd_bit, bus_width;
+
+                if (host->caps & MMC_CAP_8_BIT_DATA) {
+                        ext_csd_bit = EXT_CSD_BUS_WIDTH_8;
+                        bus_width = MMC_BUS_WIDTH_8;
+                } else {
+                        ext_csd_bit = EXT_CSD_BUS_WIDTH_4;
+                        bus_width = MMC_BUS_WIDTH_4;
+                }
+
                 err = mmc_switch(card, EXT_CSD_CMD_SET_NORMAL,
-                        EXT_CSD_BUS_WIDTH, EXT_CSD_BUS_WIDTH_4);
+                                 EXT_CSD_BUS_WIDTH, ext_csd_bit);
+
                 if (err)
                         goto free_card;
 
-                mmc_set_bus_width(card->host, MMC_BUS_WIDTH_4);
+                mmc_set_bus_width(card->host, bus_width);
         }
 
         if (!oldcard)
@@ -624,4 +635,3 @@ err:
 
         return err;
 }
-