[PATCH v2 2/3] soundwire: qcom: add support to block packing mode

[Srinivas Kandagatla]

This patch adds support to block pack mode, which is required
on Qcom soundwire controllers v1.5.x on few ports!

Signed-off-by: Srinivas Kandagatla <srinivas.kandaga...@linaro.org>
---
 drivers/soundwire/qcom.c | 22 +++++++++++++++++++---
 1 file changed, 19 insertions(+), 3 deletions(-)

diff --git a/drivers/soundwire/qcom.c b/drivers/soundwire/qcom.c
index 5d26361ab4f6..76963a7bdaa3 100644
--- a/drivers/soundwire/qcom.c
+++ b/drivers/soundwire/qcom.c
@@ -54,6 +54,7 @@
 #define SWRM_MCP_SLV_STATUS                                    0x1090
 #define SWRM_MCP_SLV_STATUS_MASK                               GENMASK(1, 0)
 #define SWRM_DP_PORT_CTRL_BANK(n, m)   (0x1124 + 0x100 * (n - 1) + 0x40 * m)
+#define SWRM_DP_BLOCK_CTRL3_BANK(n, m) (0x1138 + 0x100 * (n - 1) + 0x40 * m)
 #define SWRM_DP_PORT_CTRL_EN_CHAN_SHFT                         0x18
 #define SWRM_DP_PORT_CTRL_OFFSET2_SHFT                         0x10
 #define SWRM_DP_PORT_CTRL_OFFSET1_SHFT                         0x08
@@ -82,6 +83,7 @@ struct qcom_swrm_port_config {
        u8 si;
        u8 off1;
        u8 off2;
+       u8 bp_mode;
 };
 
 struct qcom_swrm_ctrl {
@@ -396,14 +398,22 @@ static int qcom_swrm_transport_params(struct sdw_bus *bus,
 {
        struct qcom_swrm_ctrl *ctrl = to_qcom_sdw(bus);
        u32 value;
+       int reg = SWRM_DP_PORT_CTRL_BANK((params->port_num), bank);
+       int ret;
 
        value = params->offset1 << SWRM_DP_PORT_CTRL_OFFSET1_SHFT;
        value |= params->offset2 << SWRM_DP_PORT_CTRL_OFFSET2_SHFT;
        value |= params->sample_interval - 1;
 
-       return ctrl->reg_write(ctrl,
-                              SWRM_DP_PORT_CTRL_BANK((params->port_num), bank),
-                              value);
+       ret = ctrl->reg_write(ctrl, reg, value);
+
+       if (!ret && params->blk_pkg_mode) {
+               reg = SWRM_DP_BLOCK_CTRL3_BANK(params->port_num, bank);
+
+               ret = ctrl->reg_write(ctrl, reg, 1);
+       }
+
+       return ret;
 }
 
 static int qcom_swrm_port_enable(struct sdw_bus *bus,
@@ -451,6 +461,7 @@ static int qcom_swrm_compute_params(struct sdw_bus *bus)
                        p_rt->transport_params.sample_interval = pcfg->si + 1;
                        p_rt->transport_params.offset1 = pcfg->off1;
                        p_rt->transport_params.offset2 = pcfg->off2;
+                       p_rt->transport_params.blk_pkg_mode = pcfg->bp_mode;
                }
 
                list_for_each_entry(s_rt, &m_rt->slave_rt_list, m_rt_node) {
@@ -461,6 +472,7 @@ static int qcom_swrm_compute_params(struct sdw_bus *bus)
                                        pcfg->si + 1;
                                p_rt->transport_params.offset1 = pcfg->off1;
                                p_rt->transport_params.offset2 = pcfg->off2;
+                               p_rt->transport_params.blk_pkg_mode = 
pcfg->bp_mode;
                                i++;
                        }
                }
@@ -707,6 +719,7 @@ static int qcom_swrm_get_port_config(struct qcom_swrm_ctrl 
*ctrl)
        u8 off1[QCOM_SDW_MAX_PORTS];
        u8 off2[QCOM_SDW_MAX_PORTS];
        u8 si[QCOM_SDW_MAX_PORTS];
+       u8 bp_mode[QCOM_SDW_MAX_PORTS] = { 0, };
        int i, ret, nports, val;
 
        ctrl->reg_read(ctrl, SWRM_COMP_PARAMS, &val);
@@ -749,10 +762,13 @@ static int qcom_swrm_get_port_config(struct 
qcom_swrm_ctrl *ctrl)
        if (ret)
                return ret;
 
+       ret = of_property_read_u8_array(np, "qcom,ports-block-pack-mode",
+                                       bp_mode, nports);
        for (i = 0; i < nports; i++) {
                ctrl->pconfig[i].si = si[i];
                ctrl->pconfig[i].off1 = off1[i];
                ctrl->pconfig[i].off2 = off2[i];
+               ctrl->pconfig[i].bp_mode = bp_mode[i];
        }
 
        return 0;
-- 
2.21.0