[PATCH 05/17] DRM/KMS/EDID: Fix up EEDID Map Blocks if Extension block count has changed.

[Egbert Eich]

EEDID v1.3 mandates map blocks if more than one EDID extension block
is used while in v1.4 they are optional.
If the extension count has been changed (because some extension
blocks were not readable) those map blocks need fixing.
In case of v1.4 or higher we simply eliminate all map blocks as
this is less time consuming. For v1.3 we scrap any exsisting map
blocks and recreate them from scratch.

Signed-off-by: Egbert Eich <eich at suse.de>
---
 drivers/gpu/drm/drm_edid.c |  105 +++++++++++++++++++++++++++++++++++++++++--
 1 files changed, 100 insertions(+), 5 deletions(-)

diff --git a/drivers/gpu/drm/drm_edid.c b/drivers/gpu/drm/drm_edid.c
index ec6f3bb..d1b9d67 100644
--- a/drivers/gpu/drm/drm_edid.c
+++ b/drivers/gpu/drm/drm_edid.c
@@ -44,7 +44,11 @@
 #define EDID_DETAILED_TIMINGS 4
 
 #define EDID_EXTENSION_FLAG_OFFSET  ((size_t)&((struct edid *)0)->extensions)
-#define EDID_CHECKSUM_OFFSET  ((size_t)&((struct edid *)0)->checksum)
+#define EDID_CHECKSUM_OFFSET	    ((size_t)&((struct edid *)0)->checksum)
+#define EDID_VERSION_MAJOR_OFFSET   ((size_t)&((struct edid *)0)->version)
+#define EDID_VERSION_MINOR_OFFSET   ((size_t)&((struct edid *)0)->revision)
+#define EEDID_BLOCK_MAP_FLAG      0xF0
+
 /*
  * EDID blocks out in the wild have a variety of bugs, try to collect
  * them here (note that userspace may work around broken monitors first,
@@ -318,6 +322,92 @@ static bool drm_edid_is_zero(u8 *in_edid, int length)
 	return true;
 }
 
+static void
+fix_map(u8 *block, int cnt)
+{
+	int i;
+	u8 csum;
+
+       if (--cnt > 127)
+	       cnt = 127;
+       memset(block, 0, 128);
+       block[0] = EEDID_BLOCK_MAP_FLAG;
+       csum = block[0];
+
+       for (i = 1; i < cnt; i++) {
+	       block[i] = block[i * EDID_LENGTH];
+	       csum += block[i];
+       }
+       block[127] = (u8)(0x100 - csum);
+}
+
+static int
+fixup_blockmaps(u8 **blockp, int eblock_cnt)
+{
+	int i;
+	u8 *block = *blockp;
+
+	if (block[EDID_VERSION_MAJOR_OFFSET] > 1)
+		return 0;
+	if (block[EDID_VERSION_MINOR_OFFSET] < 3)
+		return 0;
+	if (eblock_cnt == 1) {
+		if (block[EDID_LENGTH] == EEDID_BLOCK_MAP_FLAG)
+			return 0;
+		else
+			return 1;
+	}
+	if (block[EDID_VERSION_MINOR_OFFSET] >= 4) {
+		/* blockmaps are optional: simply toss them */
+		for (i = 1; i <= eblock_cnt; i++) {
+			if (block[i * EDID_LENGTH] == EEDID_BLOCK_MAP_FLAG) {
+				if (i < eblock_cnt)
+					memmove(&block[i * EDID_LENGTH],
+						&block[(i + 1) * EDID_LENGTH],
+						(eblock_cnt-i) * EDID_LENGTH);
+				i--;
+				eblock_cnt--;
+			}
+		}
+	} else {
+		int total_cnt = block[EDID_EXTENSION_FLAG_OFFSET];
+		for (i = 1; i <= eblock_cnt; i++) {
+			if (block[i * EDID_LENGTH] == EEDID_BLOCK_MAP_FLAG) {
+				if (i == 1 || i == 128) /* correct map block locations */
+					continue;
+				if (i < eblock_cnt)
+					memmove(&block[i * EDID_LENGTH],
+						&block[(i + 1) * EDID_LENGTH],
+						(eblock_cnt-i) * EDID_LENGTH);
+				i--;
+				eblock_cnt--;
+				continue;
+			} else if (i == 1 || i == 128) {
+				if (eblock_cnt >= total_cnt) {
+					u8 *tmp_p;
+					tmp_p = krealloc(block, (eblock_cnt + 2) * EDID_LENGTH, GFP_KERNEL);
+					if (!tmp_p)
+						return -ENOMEM;
+					*blockp = block = tmp_p;
+					total_cnt = eblock_cnt + 1;
+				}
+				eblock_cnt++;
+				memmove(&block[(i + 1) * EDID_LENGTH],
+					&block[i * EDID_LENGTH],
+					(eblock_cnt-i) * EDID_LENGTH);
+			}
+		}
+		fix_map(&block[EDID_LENGTH], eblock_cnt);
+
+		if (eblock_cnt == 129)
+			return 128;
+
+		if (eblock_cnt > 129)
+		       fix_map(&block[EDID_LENGTH * 128], eblock_cnt - 127);
+	}
+	return eblock_cnt;
+}
+
 static u8 *
 drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 {
@@ -385,10 +475,15 @@ drm_do_get_edid(struct drm_connector *connector, struct i2c_adapter *adapter)
 
  no_more:
 	if (valid_extensions != block[EDID_EXTENSION_FLAG_OFFSET]) {
-		block[EDID_CHECKSUM_OFFSET] += block[EDID_EXTENSION_FLAG_OFFSET] - valid_extensions;
-		block[EDID_EXTENSION_FLAG_OFFSET] = valid_extensions;
-		new = krealloc(block, (valid_extensions + 1) * EDID_LENGTH, GFP_KERNEL);
-		if (!new)
+		if (valid_extensions)
+			valid_extensions = fixup_blockmaps(&block, valid_extensions);
+		if (valid_extensions >= 0) {
+			block[EDID_CHECKSUM_OFFSET] += block[EDID_EXTENSION_FLAG_OFFSET] - valid_extensions;
+			block[EDID_EXTENSION_FLAG_OFFSET] = valid_extensions;
+			new = krealloc(block, (valid_extensions + 1) * EDID_LENGTH, GFP_KERNEL);
+			if (!new)
+				goto out;
+		} else
 			goto out;
 		block = new;
 	}
-- 
1.7.7