[media] s5p-tv: sdo: Restore vpll clock rate after streamoff

Restore vpll clock rate if start stream fail or stream is off.

Signed-off-by: Mateusz Krawczuk <m.krawczuk@partner.samsung.com>
Signed-off-by: Kyungmin Park <kyungmin.park@samsung.com>
[s.nawrocki@samsung.com: fixed whitespace error reported by checkpath.pl]
Signed-off-by: Sylwester Nawrocki <s.nawrocki@samsung.com>
Signed-off-by: Mauro Carvalho Chehab <mchehab@redhat.com>

diff --git a/drivers/media/platform/s5p-tv/sdo_drv.c b/drivers/media/platform/s5p-tv/sdo_drv.c
index 0afa90f0f6abfdfd8aee75981c147608b9c1262e..fbc6d7ae729771425a7ccdd62aa2eb9e402932bb 100644 (file)
--- a/drivers/media/platform/s5p-tv/sdo_drv.c
+++ b/drivers/media/platform/s5p-tv/sdo_drv.c
@@ -55,6 +55,8 @@ struct sdo_device {
        struct clk *dacphy;
        /** clock for control of VPLL */
        struct clk *fout_vpll;
+       /** vpll rate before sdo stream was on */
+       unsigned long vpll_rate;
        /** regulator for SDO IP power */
        struct regulator *vdac;
        /** regulator for SDO plug detection */
@@ -193,17 +195,33 @@ static int sdo_s_power(struct v4l2_subdev *sd, int on)
 
 static int sdo_streamon(struct sdo_device *sdev)
 {
+       int ret;
+
        /* set proper clock for Timing Generator */
-       clk_set_rate(sdev->fout_vpll, 54000000);
+       sdev->vpll_rate = clk_get_rate(sdev->fout_vpll);
+       ret = clk_set_rate(sdev->fout_vpll, 54000000);
+       if (ret < 0) {
+               dev_err(sdev->dev, "Failed to set vpll rate\n");
+               return ret;
+       }
        dev_info(sdev->dev, "fout_vpll.rate = %lu\n",
        clk_get_rate(sdev->fout_vpll));
        /* enable clock in SDO */
        sdo_write_mask(sdev, SDO_CLKCON, ~0, SDO_TVOUT_CLOCK_ON);
-       clk_enable(sdev->dacphy);
+       ret = clk_enable(sdev->dacphy);
+       if (ret < 0) {
+               dev_err(sdev->dev, "clk_enable(dacphy) failed\n");
+               goto fail;
+       }
        /* enable DAC */
        sdo_write_mask(sdev, SDO_DAC, ~0, SDO_POWER_ON_DAC);
        sdo_reg_debug(sdev);
        return 0;
+
+fail:
+       sdo_write_mask(sdev, SDO_CLKCON, 0, SDO_TVOUT_CLOCK_ON);
+       clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
+       return ret;
 }
 
 static int sdo_streamoff(struct sdo_device *sdev)
@@ -220,6 +238,7 @@ static int sdo_streamoff(struct sdo_device *sdev)
        }
        if (tries == 0)
                dev_err(sdev->dev, "failed to stop streaming\n");
+       clk_set_rate(sdev->fout_vpll, sdev->vpll_rate);
        return tries ? 0 : -EIO;
 }