5 files changed, 197 insertions, 22 deletions

diff --git a/drivers/gpu/drm/omapdrm/dss/dispc.c b/drivers/gpu/drm/omapdrm/dss/dispc.c
index 0aecce2c6ba0..cdd227c8bed3 100644
--- a/drivers/gpu/drm/omapdrm/dss/dispc.c
+++ b/drivers/gpu/drm/omapdrm/dss/dispc.c
@@ -112,9 +112,12 @@ struct dispc_features {
          * never both, we can just use this flag for now.
          */
         bool reverse_ilace_field_order:1;
+
+        bool has_gamma_table:1;
 };
 
 #define DISPC_MAX_NR_FIFOS 5
+#define DISPC_MAX_CHANNEL_GAMMA 4
 
 static struct {
         struct platform_device *pdev;
@@ -134,6 +137,8 @@ static struct {
         bool            ctx_valid;
         u32             ctx[DISPC_SZ_REGS / sizeof(u32)];
 
+        u32 *gamma_table[DISPC_MAX_CHANNEL_GAMMA];
+
         const struct dispc_features *feat;
 
         bool is_enabled;
@@ -177,11 +182,19 @@ struct dispc_reg_field {
         u8 low;
 };
 
+struct dispc_gamma_desc {
+        u32 len;
+        u32 bits;
+        u16 reg;
+        bool has_index;
+};
+
 static const struct {
         const char *name;
         u32 vsync_irq;
         u32 framedone_irq;
         u32 sync_lost_irq;
+        struct dispc_gamma_desc gamma;
         struct dispc_reg_field reg_desc[DISPC_MGR_FLD_NUM];
 } mgr_desc[] = {
         [OMAP_DSS_CHANNEL_LCD] = {
@@ -189,6 +202,12 @@ static const struct {
                 .vsync_irq      = DISPC_IRQ_VSYNC,
                 .framedone_irq  = DISPC_IRQ_FRAMEDONE,
                 .sync_lost_irq  = DISPC_IRQ_SYNC_LOST,
+                .gamma          = {
+                        .len    = 256,
+                        .bits   = 8,
+                        .reg    = DISPC_GAMMA_TABLE0,
+                        .has_index = true,
+                },
                 .reg_desc       = {
                         [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL,  0,  0 },
                         [DISPC_MGR_FLD_STNTFT]          = { DISPC_CONTROL,  3,  3 },
@@ -206,6 +225,12 @@ static const struct {
                 .vsync_irq      = DISPC_IRQ_EVSYNC_ODD | DISPC_IRQ_EVSYNC_EVEN,
                 .framedone_irq  = DISPC_IRQ_FRAMEDONETV,
                 .sync_lost_irq  = DISPC_IRQ_SYNC_LOST_DIGIT,
+                .gamma          = {
+                        .len    = 1024,
+                        .bits   = 10,
+                        .reg    = DISPC_GAMMA_TABLE2,
+                        .has_index = false,
+                },
                 .reg_desc       = {
                         [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL,  1,  1 },
                         [DISPC_MGR_FLD_STNTFT]          = { },
@@ -223,6 +248,12 @@ static const struct {
                 .vsync_irq      = DISPC_IRQ_VSYNC2,
                 .framedone_irq  = DISPC_IRQ_FRAMEDONE2,
                 .sync_lost_irq  = DISPC_IRQ_SYNC_LOST2,
+                .gamma          = {
+                        .len    = 256,
+                        .bits   = 8,
+                        .reg    = DISPC_GAMMA_TABLE1,
+                        .has_index = true,
+                },
                 .reg_desc       = {
                         [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL2,  0,  0 },
                         [DISPC_MGR_FLD_STNTFT]          = { DISPC_CONTROL2,  3,  3 },
@@ -240,6 +271,12 @@ static const struct {
                 .vsync_irq      = DISPC_IRQ_VSYNC3,
                 .framedone_irq  = DISPC_IRQ_FRAMEDONE3,
                 .sync_lost_irq  = DISPC_IRQ_SYNC_LOST3,
+                .gamma          = {
+                        .len    = 256,
+                        .bits   = 8,
+                        .reg    = DISPC_GAMMA_TABLE3,
+                        .has_index = true,
+                },
                 .reg_desc       = {
                         [DISPC_MGR_FLD_ENABLE]          = { DISPC_CONTROL3,  0,  0 },
                         [DISPC_MGR_FLD_STNTFT]          = { DISPC_CONTROL3,  3,  3 },
@@ -1083,20 +1120,6 @@ static u32 dispc_ovl_get_burst_size(enum omap_plane plane)
         return unit * 8;
 }
 
-void dispc_enable_gamma_table(bool enable)
-{
-        /*
-         * This is partially implemented to support only disabling of
-         * the gamma table.
-         */
-        if (enable) {
-                DSSWARN("Gamma table enabling for TV not yet supported");
-                return;
-        }
-
-        REG_FLD_MOD(DISPC_CONFIG, enable, 9, 9);
-}
-
 static void dispc_mgr_enable_cpr(enum omap_channel channel, bool enable)
 {
         if (channel == OMAP_DSS_CHANNEL_DIGIT)
@@ -3790,6 +3813,139 @@ void dispc_disable_sidle(void)
         REG_FLD_MOD(DISPC_SYSCONFIG, 1, 4, 3);  /* SIDLEMODE: no idle */
 }
 
+u32 dispc_mgr_gamma_size(enum omap_channel channel)
+{
+        const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+
+        if (!dispc.feat->has_gamma_table)
+                return 0;
+
+        return gdesc->len;
+}
+EXPORT_SYMBOL(dispc_mgr_gamma_size);
+
+static void dispc_mgr_write_gamma_table(enum omap_channel channel)
+{
+        const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+        u32 *table = dispc.gamma_table[channel];
+        unsigned int i;
+
+        DSSDBG("%s: channel %d\n", __func__, channel);
+
+        for (i = 0; i < gdesc->len; ++i) {
+                u32 v = table[i];
+
+                if (gdesc->has_index)
+                        v |= i << 24;
+                else if (i == 0)
+                        v |= 1 << 31;
+
+                dispc_write_reg(gdesc->reg, v);
+        }
+}
+
+static void dispc_restore_gamma_tables(void)
+{
+        DSSDBG("%s()\n", __func__);
+
+        if (!dispc.feat->has_gamma_table)
+                return;
+
+        dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_LCD);
+
+        dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_DIGIT);
+
+        if (dss_has_feature(FEAT_MGR_LCD2))
+                dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_LCD2);
+
+        if (dss_has_feature(FEAT_MGR_LCD3))
+                dispc_mgr_write_gamma_table(OMAP_DSS_CHANNEL_LCD3);
+}
+
+static const struct drm_color_lut dispc_mgr_gamma_default_lut[] = {
+        { .red = 0, .green = 0, .blue = 0, },
+        { .red = U16_MAX, .green = U16_MAX, .blue = U16_MAX, },
+};
+
+void dispc_mgr_set_gamma(enum omap_channel channel,
+                         const struct drm_color_lut *lut,
+                         unsigned int length)
+{
+        const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+        u32 *table = dispc.gamma_table[channel];
+        uint i;
+
+        DSSDBG("%s: channel %d, lut len %u, hw len %u\n", __func__,
+               channel, length, gdesc->len);
+
+        if (!dispc.feat->has_gamma_table)
+                return;
+
+        if (lut == NULL || length < 2) {
+                lut = dispc_mgr_gamma_default_lut;
+                length = ARRAY_SIZE(dispc_mgr_gamma_default_lut);
+        }
+
+        for (i = 0; i < length - 1; ++i) {
+                uint first = i * (gdesc->len - 1) / (length - 1);
+                uint last = (i + 1) * (gdesc->len - 1) / (length - 1);
+                uint w = last - first;
+                u16 r, g, b;
+                uint j;
+
+                if (w == 0)
+                        continue;
+
+                for (j = 0; j <= w; j++) {
+                        r = (lut[i].red * (w - j) + lut[i+1].red * j) / w;
+                        g = (lut[i].green * (w - j) + lut[i+1].green * j) / w;
+                        b = (lut[i].blue * (w - j) + lut[i+1].blue * j) / w;
+
+                        r >>= 16 - gdesc->bits;
+                        g >>= 16 - gdesc->bits;
+                        b >>= 16 - gdesc->bits;
+
+                        table[first + j] = (r << (gdesc->bits * 2)) |
+                                (g << gdesc->bits) | b;
+                }
+        }
+
+        if (dispc.is_enabled)
+                dispc_mgr_write_gamma_table(channel);
+}
+EXPORT_SYMBOL(dispc_mgr_set_gamma);
+
+static int dispc_init_gamma_tables(void)
+{
+        int channel;
+
+        if (!dispc.feat->has_gamma_table)
+                return 0;
+
+        for (channel = 0; channel < ARRAY_SIZE(dispc.gamma_table); channel++) {
+                const struct dispc_gamma_desc *gdesc = &mgr_desc[channel].gamma;
+                u32 *gt;
+
+                if (channel == OMAP_DSS_CHANNEL_LCD2 &&
+                    !dss_has_feature(FEAT_MGR_LCD2))
+                        continue;
+
+                if (channel == OMAP_DSS_CHANNEL_LCD3 &&
+                    !dss_has_feature(FEAT_MGR_LCD3))
+                        continue;
+
+                gt = devm_kmalloc_array(&dispc.pdev->dev, gdesc->len,
+                                           sizeof(u32), GFP_KERNEL);
+                if (!gt)
+                        return -ENOMEM;
+
+                dispc.gamma_table[channel] = gt;
+
+                dispc_mgr_set_gamma(channel, NULL, 0);
+        }
+        return 0;
+}
+
 static void _omap_dispc_initial_config(void)
 {
         u32 l;
@@ -3805,8 +3961,15 @@ static void _omap_dispc_initial_config(void)
                 dispc.core_clk_rate = dispc_fclk_rate();
         }
 
-        /* FUNCGATED */
-        if (dss_has_feature(FEAT_FUNCGATED))
+        /* Use gamma table mode, instead of palette mode */
+        if (dispc.feat->has_gamma_table)
+                REG_FLD_MOD(DISPC_CONFIG, 1, 3, 3);
+
+        /* For older DSS versions (FEAT_FUNCGATED) this enables
+         * func-clock auto-gating. For newer versions
+         * (dispc.feat->has_gamma_table) this enables tv-out gamma tables.
+         */
+        if (dss_has_feature(FEAT_FUNCGATED) || dispc.feat->has_gamma_table)
                 REG_FLD_MOD(DISPC_CONFIG, 1, 9, 9);
 
         dispc_setup_color_conv_coef();
@@ -3910,6 +4073,7 @@ static const struct dispc_features omap44xx_dispc_feats = {
         .has_writeback          =       true,
         .supports_double_pixel  =       true,
         .reverse_ilace_field_order =    true,
+        .has_gamma_table        =       true,
 };
 
 static const struct dispc_features omap54xx_dispc_feats = {
@@ -3935,6 +4099,7 @@ static const struct dispc_features omap54xx_dispc_feats = {
         .has_writeback          =       true,
         .supports_double_pixel  =       true,
         .reverse_ilace_field_order =    true,
+        .has_gamma_table        =       true,
 };
 
 static int dispc_init_features(struct platform_device *pdev)
@@ -4076,6 +4241,10 @@ static int dispc_bind(struct device *dev, struct device *master, void *data)
                 }
         }
 
+        r = dispc_init_gamma_tables();
+        if (r)
+                return r;
+
         pm_runtime_enable(&pdev->dev);
 
         r = dispc_runtime_get();
@@ -4146,6 +4315,8 @@ static int dispc_runtime_resume(struct device *dev)
                 _omap_dispc_initial_config();
 
                 dispc_restore_context();
+
+                dispc_restore_gamma_tables();
         }
 
         dispc.is_enabled = true;
diff --git a/drivers/gpu/drm/omapdrm/dss/dispc.h b/drivers/gpu/drm/omapdrm/dss/dispc.h
index 483744223dd1..bc1d8126ee87 100644
--- a/drivers/gpu/drm/omapdrm/dss/dispc.h
+++ b/drivers/gpu/drm/omapdrm/dss/dispc.h
@@ -42,6 +42,11 @@
 #define DISPC_MSTANDBY_CTRL             0x0858
 #define DISPC_GLOBAL_MFLAG_ATTRIBUTE    0x085C
 
+#define DISPC_GAMMA_TABLE0              0x0630
+#define DISPC_GAMMA_TABLE1              0x0634
+#define DISPC_GAMMA_TABLE2              0x0638
+#define DISPC_GAMMA_TABLE3              0x0850
+
 /* DISPC overlay registers */
 #define DISPC_OVL_BA0(n)                (DISPC_OVL_BASE(n) + \
                                         DISPC_BA0_OFFSET(n))
diff --git a/drivers/gpu/drm/omapdrm/dss/hdmi4.c b/drivers/gpu/drm/omapdrm/dss/hdmi4.c
index 2cfd70dee1b2..cbd28dfdb86a 100644
--- a/drivers/gpu/drm/omapdrm/dss/hdmi4.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi4.c
@@ -208,9 +208,6 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 
         hdmi4_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
 
-        /* bypass TV gamma table */
-        dispc_enable_gamma_table(0);
-
         /* tv size */
         dss_mgr_set_timings(channel, p);
 
diff --git a/drivers/gpu/drm/omapdrm/dss/hdmi5.c b/drivers/gpu/drm/omapdrm/dss/hdmi5.c
index d4892d84424e..061f9bab4c9b 100644
--- a/drivers/gpu/drm/omapdrm/dss/hdmi5.c
+++ b/drivers/gpu/drm/omapdrm/dss/hdmi5.c
@@ -226,9 +226,6 @@ static int hdmi_power_on_full(struct omap_dss_device *dssdev)
 
         hdmi5_configure(&hdmi.core, &hdmi.wp, &hdmi.cfg);
 
-        /* bypass TV gamma table */
-        dispc_enable_gamma_table(0);
-
         /* tv size */
         dss_mgr_set_timings(channel, p);
 
diff --git a/drivers/gpu/drm/omapdrm/dss/omapdss.h b/drivers/gpu/drm/omapdrm/dss/omapdss.h
index 9263283952b9..6eaf1adbd606 100644
--- a/drivers/gpu/drm/omapdrm/dss/omapdss.h
+++ b/drivers/gpu/drm/omapdrm/dss/omapdss.h
@@ -24,6 +24,7 @@
 #include <linux/interrupt.h>
 #include <video/videomode.h>
 #include <linux/platform_data/omapdss.h>
+#include <uapi/drm/drm_mode.h>
 
 #define DISPC_IRQ_FRAMEDONE             (1 << 0)
 #define DISPC_IRQ_VSYNC                 (1 << 1)
@@ -908,6 +909,10 @@ void dispc_mgr_set_timings(enum omap_channel channel,
                 const struct omap_video_timings *timings);
 void dispc_mgr_setup(enum omap_channel channel,
                 const struct omap_overlay_manager_info *info);
+u32 dispc_mgr_gamma_size(enum omap_channel channel);
+void dispc_mgr_set_gamma(enum omap_channel channel,
+                         const struct drm_color_lut *lut,
+                         unsigned int length);
 
 int dispc_ovl_enable(enum omap_plane plane, bool enable);
 bool dispc_ovl_enabled(enum omap_plane plane);