mfd: rtsx: Add clock divider hook

Add callback function conv_clk_and_div_n to convert between SSC clock
and its divider N.
For rtl8411, the formula to calculate SSC clock divider N is different
with the other card reader models.

Signed-off-by: Wei WANG <wei_wang@realsil.com.cn>
Signed-off-by: Samuel Ortiz <sameo@linux.intel.com>

4 files changed, 27 insertions, 2 deletions

diff --git a/drivers/mfd/rtl8411.c b/drivers/mfd/rtl8411.c
index 5058ba8dec3b..3d3b4addf81a 100644
--- a/drivers/mfd/rtl8411.c
+++ b/drivers/mfd/rtl8411.c
@@ -178,6 +178,18 @@ static unsigned int rtl8411_cd_deglitch(struct rtsx_pcr *pcr)
         return card_exist;
 }
 
+static int rtl8411_conv_clk_and_div_n(int input, int dir)
+{
+        int output;
+
+        if (dir == CLK_TO_DIV_N)
+                output = input * 4 / 5 - 2;
+        else
+                output = (input + 2) * 5 / 4;
+
+        return output;
+}
+
 static const struct pcr_ops rtl8411_pcr_ops = {
         .extra_init_hw = rtl8411_extra_init_hw,
         .optimize_phy = NULL,
@@ -189,6 +201,7 @@ static const struct pcr_ops rtl8411_pcr_ops = {
         .card_power_off = rtl8411_card_power_off,
         .switch_output_voltage = rtl8411_switch_output_voltage,
         .cd_deglitch = rtl8411_cd_deglitch,
+        .conv_clk_and_div_n = rtl8411_conv_clk_and_div_n,
 };
 
 /* SD Pull Control Enable:
diff --git a/drivers/mfd/rts5209.c b/drivers/mfd/rts5209.c
index ba74de8a7c24..98fe0f39463e 100644
--- a/drivers/mfd/rts5209.c
+++ b/drivers/mfd/rts5209.c
@@ -174,6 +174,7 @@ static const struct pcr_ops rts5209_pcr_ops = {
         .card_power_off = rts5209_card_power_off,
         .switch_output_voltage = rts5209_switch_output_voltage,
         .cd_deglitch = NULL,
+        .conv_clk_and_div_n = NULL,
 };
 
 /* SD Pull Control Enable:
diff --git a/drivers/mfd/rts5229.c b/drivers/mfd/rts5229.c
index ec1747adf5d3..29d889cbb9c5 100644
--- a/drivers/mfd/rts5229.c
+++ b/drivers/mfd/rts5229.c
@@ -144,6 +144,7 @@ static const struct pcr_ops rts5229_pcr_ops = {
         .card_power_off = rts5229_card_power_off,
         .switch_output_voltage = rts5229_switch_output_voltage,
         .cd_deglitch = NULL,
+        .conv_clk_and_div_n = NULL,
 };
 
 /* SD Pull Control Enable:
diff --git a/drivers/mfd/rtsx_pcr.c b/drivers/mfd/rtsx_pcr.c
index f4e02d089271..910200e10fa0 100644
--- a/drivers/mfd/rtsx_pcr.c
+++ b/drivers/mfd/rtsx_pcr.c
@@ -630,7 +630,10 @@ int rtsx_pci_switch_clock(struct rtsx_pcr *pcr, unsigned int card_clock,
         if (clk == pcr->cur_clock)
                 return 0;
 
-        N = (u8)(clk - 2);
+        if (pcr->ops->conv_clk_and_div_n)
+                N = (u8)pcr->ops->conv_clk_and_div_n(clk, CLK_TO_DIV_N);
+        else
+                N = (u8)(clk - 2);
         if ((clk <= 2) || (N > max_N))
                 return -EINVAL;
 
@@ -641,7 +644,14 @@ int rtsx_pci_switch_clock(struct rtsx_pcr *pcr, unsigned int card_clock,
         /* Make sure that the SSC clock div_n is equal or greater than min_N */
         div = CLK_DIV_1;
         while ((N < min_N) && (div < max_div)) {
-                N = (N + 2) * 2 - 2;
+                if (pcr->ops->conv_clk_and_div_n) {
+                        int dbl_clk = pcr->ops->conv_clk_and_div_n(N,
+                                        DIV_N_TO_CLK) * 2;
+                        N = (u8)pcr->ops->conv_clk_and_div_n(dbl_clk,
+                                        CLK_TO_DIV_N);
+                } else {
+                        N = (N + 2) * 2 - 2;
+                }
                 div++;
         }
         dev_dbg(&(pcr->pci->dev), "N = %d, div = %d\n", N, div);