1 files changed, 96 insertions, 0 deletions

diff --git a/util/cbfstool/platform_fixups.c b/util/cbfstool/platform_fixups.c
index ea2d3161a2..b2e12cf6a2 100644
--- a/util/cbfstool/platform_fixups.c
+++ b/util/cbfstool/platform_fixups.c
@@ -1,5 +1,8 @@
 /* SPDX-License-Identifier: GPL-2.0-only */
 
+#include <commonlib/endian.h>
+#include <string.h>
+
 #include "cbfs.h"
 #include "cbfs_sections.h"
 #include "elfparsing.h"
@@ -105,12 +108,105 @@ static int qualcomm_fixup(struct buffer *buffer, size_t offset)
 	return 0;
 }
 
+/*
+ * MediaTek bootblock.bin layout (see util/mtkheader/gen-bl-img.py):
+ *	header		2048 bytes
+ *	gfh info	176 bytes, where bytes 32-35 (in little endian) is the
+ *			total size excluding the header (gfh info + data + hash)
+ *	data		`data_size` bytes
+ *	hash		32 bytes, SHA256 of "gfh info + data"
+ *	padding
+ */
+#define MEDIATEK_BOOTBLOCK_HEADER_SIZE	2048
+#define MEDIATEK_BOOTBLOCK_GFH_SIZE	176
+static void *mediatek_find_hash(struct buffer *bootblock, struct vb2_hash *real_hash)
+{
+	struct buffer buffer;
+	size_t data_size;
+	const char emmc_magic[] = "EMMC_BOOT";
+	const char sf_magic[] = "SF_BOOT";
+	const char brlyt_magic[] = "BRLYT";
+	const size_t brlyt_offset = 512;
+
+	buffer_clone(&buffer, bootblock);
+
+	/* Doesn't seem to be MediaTek image */
+	if (buffer_size(&buffer) <
+	    MEDIATEK_BOOTBLOCK_HEADER_SIZE + MEDIATEK_BOOTBLOCK_GFH_SIZE)
+		return NULL;
+
+	/* Check header magic */
+	if (!buffer_check_magic(&buffer, emmc_magic, strlen(emmc_magic)) &&
+	    !buffer_check_magic(&buffer, sf_magic, strlen(sf_magic)))
+		return NULL;
+
+	/* Check "BRLYT" */
+	buffer_seek(&buffer, brlyt_offset);
+	if (!buffer_check_magic(&buffer, brlyt_magic, strlen(brlyt_magic)))
+		return NULL;
+
+	buffer_seek(&buffer, MEDIATEK_BOOTBLOCK_HEADER_SIZE - brlyt_offset);
+	data_size = read_le32(buffer_get(&buffer) + 32);
+	if (data_size <= MEDIATEK_BOOTBLOCK_GFH_SIZE + VB2_SHA256_DIGEST_SIZE) {
+		ERROR("fixups: MediaTek: data size too small: %zu\n", data_size);
+		return NULL;
+	}
+	data_size -= MEDIATEK_BOOTBLOCK_GFH_SIZE + VB2_SHA256_DIGEST_SIZE;
+
+	if (buffer_size(&buffer) <
+	    MEDIATEK_BOOTBLOCK_GFH_SIZE + data_size + VB2_SHA256_DIGEST_SIZE) {
+		ERROR("fixups: MediaTek: not enough data: %zu\n", buffer_size(&buffer));
+		return NULL;
+	}
+
+	if (vb2_hash_calculate(buffer_get(&buffer),
+			       MEDIATEK_BOOTBLOCK_GFH_SIZE + data_size,
+			       VB2_HASH_SHA256, real_hash)) {
+		ERROR("fixups: MediaTek: vboot digest error\n");
+		return NULL;
+	}
+
+	buffer_seek(&buffer, MEDIATEK_BOOTBLOCK_GFH_SIZE + data_size);
+	return buffer_get(&buffer);
+}
+
+static bool mediatek_probe(struct buffer *buffer)
+{
+	struct vb2_hash real_hash;
+	void *hash = mediatek_find_hash(buffer, &real_hash);
+	if (!hash)
+		return false;
+
+	if (memcmp(real_hash.raw, hash, VB2_SHA256_DIGEST_SIZE)) {
+		ERROR("fixups: Found MediaTek bootblock, but existing hash doesn't match!\n");
+		return false;
+	}
+
+	return true;
+}
+
+static int mediatek_fixup(struct buffer *buffer, unused size_t offset)
+{
+	struct vb2_hash real_hash;
+	void *hash = mediatek_find_hash(buffer, &real_hash);
+	if (!hash) {
+		ERROR("fixups: Cannot find MediaTek header anymore!\n");
+		return -1;
+	}
+
+	memcpy(hash, real_hash.raw, VB2_SHA256_DIGEST_SIZE);
+	INFO("fixups: Updated MediaTek bootblock hash.\n");
+	return 0;
+}
+
 platform_fixup_func platform_fixups_probe(struct buffer *buffer, size_t offset,
 					  const char *region_name)
 {
 	if (!strcmp(region_name, SECTION_NAME_BOOTBLOCK)) {
 		if (qualcomm_probe(buffer, offset))
 			return qualcomm_fixup;
+		else if (mediatek_probe(buffer))
+			return mediatek_fixup;
 	} else if (!strcmp(region_name, SECTION_NAME_PRIMARY_CBFS)) {
 		/* TODO: add fixups for primary CBFS bootblock platforms, if needed */
 	} else {