slimbus: qcom-ngd-ctrl: add support for 44.1 Khz frequency

[sfrench/cifs-2.6.git] / drivers / slimbus / qcom-ngd-ctrl.c

diff --git a/drivers/slimbus/qcom-ngd-ctrl.c b/drivers/slimbus/qcom-ngd-ctrl.c
index d48e58ca5d58527656fb59a053b6b9b0abfbd169..22720ad4c22d7d21f063c1433b0a0ce403ac80bb 100644 (file)
--- a/drivers/slimbus/qcom-ngd-ctrl.c
+++ b/drivers/slimbus/qcom-ngd-ctrl.c
@@ -944,6 +944,54 @@ pm_put:
        return ret;
 }
 
+static int qcom_slim_calc_coef(struct slim_stream_runtime *rt, int *exp)
+{
+       struct slim_controller *ctrl = rt->dev->ctrl;
+       int coef;
+
+       if (rt->ratem * ctrl->a_framer->superfreq < rt->rate)
+               rt->ratem++;
+
+       coef = rt->ratem;
+       *exp = 0;
+
+       /*
+        * CRM = Cx(2^E) is the formula we are using.
+        * Here C is the coffecient and E is the exponent.
+        * CRM is the Channel Rate Multiplier.
+        * Coefficeint should be either 1 or 3 and exponenet
+        * should be an integer between 0 to 9, inclusive.
+        */
+       while (1) {
+               while ((coef & 0x1) != 0x1) {
+                       coef >>= 1;
+                       *exp = *exp + 1;
+               }
+
+               if (coef <= 3)
+                       break;
+
+               coef++;
+       }
+
+       /*
+        * we rely on the coef value (1 or 3) to set a bit
+        * in the slimbus message packet. This bit is
+        * BIT(5) which is the segment rate coefficient.
+        */
+       if (coef == 1) {
+               if (*exp > 9)
+                       return -EIO;
+               coef = 0;
+       } else {
+               if (*exp > 8)
+                       return -EIO;
+               coef = 1;
+       }
+
+       return coef;
+}
+
 static int qcom_slim_ngd_enable_stream(struct slim_stream_runtime *rt)
 {
        struct slim_device *sdev = rt->dev;
@@ -967,16 +1015,22 @@ static int qcom_slim_ngd_enable_stream(struct slim_stream_runtime *rt)
                struct slim_port *port = &rt->ports[i];
 
                if (txn.msg->num_bytes == 0) {
-                       int seg_interval = SLIM_SLOTS_PER_SUPERFRAME/rt->ratem;
-                       int exp;
+                       int exp = 0, coef = 0;
 
                        wbuf[txn.msg->num_bytes++] = sdev->laddr;
                        wbuf[txn.msg->num_bytes] = rt->bps >> 2 |
                                                   (port->ch.aux_fmt << 6);
 
-                       /* Data channel segment interval not multiple of 3 */
-                       exp = seg_interval % 3;
-                       if (exp)
+                       /* calculate coef dynamically */
+                       coef = qcom_slim_calc_coef(rt, &exp);
+                       if (coef < 0) {
+                               dev_err(&sdev->dev,
+                               "%s: error calculating coef %d\n", __func__,
+                                                                       coef);
+                               return -EIO;
+                       }
+
+                       if (coef)
                                wbuf[txn.msg->num_bytes] |= BIT(5);
 
                        txn.msg->num_bytes++;