rk3066B: support rk610 hdmi.

[firefly-linux-kernel-4.4.55.git] / drivers / video / fbmon.c

diff --git a/drivers/video/fbmon.c b/drivers/video/fbmon.c
index 18dad1dd9fb7ba8b96f2dffc041742a8660c4c17..4f57485f8c54580a30519add3a26d72779d9ebcd 100644 (file)
--- a/drivers/video/fbmon.c
+++ b/drivers/video/fbmon.c
@@ -973,97 +973,88 @@ void fb_edid_to_monspecs(unsigned char *edid, struct fb_monspecs *specs)
        DPRINTK("========================================\n");
 }
 
+/**
+ * fb_edid_add_monspecs() - add monitor video modes from E-EDID data
+ * @edid:      128 byte array with an E-EDID block
+ * @spacs:     monitor specs to be extended
+ */
 void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 {
        unsigned char *block;
-       unsigned char *dtd_block;
-       struct fb_videomode *mode, *m;
-       int num = 0, i, first = 1;
+       struct fb_videomode *m;
+       int num = 0, i;
+       u8 svd[64], edt[(128 - 4) / DETAILED_TIMING_DESCRIPTION_SIZE];
+       u8 pos = 4, svd_n = 0;
 
-       if (edid == NULL)
+       if (!edid)
                return;
 
        if (!edid_checksum(edid))
                return;
 
-       if (edid[0] != 0x2)
+       if (edid[0] != 0x2 ||
+           edid[2] < 4 || edid[2] > 128 - DETAILED_TIMING_DESCRIPTION_SIZE)
                return;
 
-       mode = kzalloc(50 * sizeof(struct fb_videomode), GFP_KERNEL);
-       if (mode == NULL)
-               return;
-
-       block = edid + 0x4;
-       dtd_block = edid + edid[0x2];
-
-       DPRINTK("  Short Video Modes\n");
-       while (block < dtd_block) {
-               unsigned tag = block[0] >> 5;
-               unsigned len = block[0] & 0x1f;
-
-               block++;
-               if (dtd_block - block < len)
-                       break;
-
-               if (tag == 0x2) {
-                       for (i = 0; i < len; i++) {
-                               unsigned m = block[i];
-                               if (m > 0 && m < CEA_MODEDB_SIZE) {
-                                       memcpy(&mode[num], &cea_modes[m],
-                                              sizeof(mode[num]));
-                                       DPRINTK("  %d: %dx%d @ %d\n", m,
-                                               cea_modes[m].xres, cea_modes[m].yres,
-                                               cea_modes[m].refresh);
-
-                                       num++;
-                               }
+       DPRINTK("  Short Video Descriptors\n");
+
+       while (pos < edid[2]) {
+               u8 len = edid[pos] & 0x1f, type = (edid[pos] >> 5) & 7;
+               pr_debug("Data block %u of %u bytes\n", type, len);
+               if (type == 2)
+                       for (i = pos; i < pos + len; i++) {
+                               u8 idx = edid[pos + i] & 0x7f;
+                               svd[svd_n++] = idx;
+                               pr_debug("N%sative mode #%d\n",
+                                        edid[pos + i] & 0x80 ? "" : "on-n", idx);
                        }
-               } else if (tag == 0x3) {
-                       if (len >= 3) {
-                               u32 ieee_reg = block[0] | (block[1] << 8) |
-                                       (block[2] << 16);
-                               if (ieee_reg == 0x000c03)
-                                       specs->misc |= FB_MISC_HDMI;
-                       }
-               }
-
-               block += len;
+               pos += len + 1;
        }
 
+       block = edid + edid[2];
+
        DPRINTK("  Extended Detailed Timings\n");
 
-       for (i = 0; i < (128 - edid[0x2]) / DETAILED_TIMING_DESCRIPTION_SIZE;
-            i++, dtd_block += DETAILED_TIMING_DESCRIPTION_SIZE) {
-               if (!(dtd_block[0] == 0x00 && dtd_block[1] == 0x00)) {
-                       get_detailed_timing(dtd_block, &mode[num]);
-                       if (first) {
-                               mode[num].flag |= FB_MODE_IS_FIRST;
-                               first = 0;
-                       }
-                       num++;
-               }
-       }
+       for (i = 0; i < (128 - edid[2]) / DETAILED_TIMING_DESCRIPTION_SIZE;
+            i++, block += DETAILED_TIMING_DESCRIPTION_SIZE)
+               if (PIXEL_CLOCK)
+                       edt[num++] = block - edid;
 
        /* Yikes, EDID data is totally useless */
-       if (!num) {
-               kfree(mode);
+       if (!(num + svd_n))
                return;
-       }
 
-       m = kzalloc((specs->modedb_len + num) *
+       m = kzalloc((specs->modedb_len + num + svd_n) *
                       sizeof(struct fb_videomode), GFP_KERNEL);
 
-       if (!m) {
-               kfree(mode);
+       if (!m)
                return;
+
+       memcpy(m, specs->modedb, specs->modedb_len * sizeof(struct fb_videomode));
+
+       for (i = specs->modedb_len; i < specs->modedb_len + num; i++) {
+               get_detailed_timing(edid + edt[i - specs->modedb_len], &m[i]);
+               if (i == specs->modedb_len)
+                       m[i].flag |= FB_MODE_IS_FIRST;
+               pr_debug("Adding %ux%u@%u\n", m[i].xres, m[i].yres, m[i].refresh);
+       }
+
+       for (i = specs->modedb_len + num; i < specs->modedb_len + num + svd_n; i++) {
+               int idx = svd[i - specs->modedb_len - num];
+               if (!idx || idx > 63) {
+                       pr_warning("Reserved SVD code %d\n", idx);
+               } else if (idx > ARRAY_SIZE(cea_modes) || !cea_modes[idx].xres) {
+                       pr_warning("Unimplemented SVD code %d\n", idx);
+               } else {
+                       memcpy(&m[i], cea_modes + idx, sizeof(m[i]));
+                       pr_debug("Adding SVD #%d: %ux%u@%u\n", idx,
+                                m[i].xres, m[i].yres, m[i].refresh);
+               }
        }
 
-       memmove(m, specs->modedb, specs->modedb_len * sizeof(struct fb_videomode));
-       memmove(m + specs->modedb_len, mode, num * sizeof(struct fb_videomode));
-       kfree(mode);
        kfree(specs->modedb);
        specs->modedb = m;
-       specs->modedb_len = specs->modedb_len + num;
+       specs->modedb_len = specs->modedb_len + num + svd_n;
 }
 
 /*
@@ -1382,6 +1373,9 @@ void fb_edid_to_monspecs(unsigned char *edid, struct fb_monspecs *specs)
 {
        specs = NULL;
 }
+void fb_edid_add_monspecs(unsigned char *edid, struct fb_monspecs *specs)
+{
+}
 void fb_destroy_modedb(struct fb_videomode *modedb)
 {
 }
@@ -1489,6 +1483,7 @@ EXPORT_SYMBOL(fb_firmware_edid);
 
 EXPORT_SYMBOL(fb_parse_edid);
 EXPORT_SYMBOL(fb_edid_to_monspecs);
+EXPORT_SYMBOL(fb_edid_add_monspecs);
 EXPORT_SYMBOL(fb_get_mode);
 EXPORT_SYMBOL(fb_validate_mode);
 EXPORT_SYMBOL(fb_destroy_modedb);