OMAPDSS: DSI: fix lane handling when entering ULPS

The current code always enters ULPS for 3 lanes. This is not right, as
there could be only 2 lanes used, and on OMAP4 we have 5 lanes.

Fix the code to put all used lanes into ULPS.

Signed-off-by: Tomi Valkeinen <tomi.valkeinen@ti.com>

diff --git a/drivers/video/omap2/dss/dsi.c b/drivers/video/omap2/dss/dsi.c
index 2b375c6..22055db 100644 (file)
--- a/drivers/video/omap2/dss/dsi.c
+++ b/drivers/video/omap2/dss/dsi.c
@@ -3517,7 +3517,8 @@ static int dsi_enter_ulps(struct platform_device *dsidev)
 {
        struct dsi_data *dsi = dsi_get_dsidrv_data(dsidev);
        DECLARE_COMPLETION_ONSTACK(completion);
-       int r;
+       int r, i;
+       unsigned mask;
 
        DSSDBGF();
 
@@ -3560,10 +3561,16 @@ static int dsi_enter_ulps(struct platform_device *dsidev)
        if (r)
                return r;
 
+       mask = 0;
+
+       for (i = 0; i < dsi->num_lanes_supported; ++i) {
+               if (dsi->lanes[i].function == DSI_LANE_UNUSED)
+                       continue;
+               mask |= 1 << i;
+       }
        /* Assert TxRequestEsc for data lanes and TxUlpsClk for clk lane */
        /* LANEx_ULPS_SIG2 */
-       REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, (1 << 0) | (1 << 1) | (1 << 2),
-               7, 5);
+       REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, mask, 9, 5);
 
        /* flush posted write and wait for SCP interface to finish the write */
        dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG2);
@@ -3579,8 +3586,7 @@ static int dsi_enter_ulps(struct platform_device *dsidev)
                        DSI_CIO_IRQ_ULPSACTIVENOT_ALL0);
 
        /* Reset LANEx_ULPS_SIG2 */
-       REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, (0 << 0) | (0 << 1) | (0 << 2),
-               7, 5);
+       REG_FLD_MOD(dsidev, DSI_COMPLEXIO_CFG2, 0, 9, 5);
 
        /* flush posted write and wait for SCP interface to finish the write */
        dsi_read_reg(dsidev, DSI_COMPLEXIO_CFG2);