From 23ca19acf84f912d1dfd3b3c3e92fd8f12f739d3 Mon Sep 17 00:00:00 2001
From: Benson Leung <bleung@chromium.org>
Date: Wed, 3 Jun 2020 13:32:59 -0700
Subject: [PATCH] fmap-firmware: Parse flashmap format into images

Implements a search for the fmap, and follow the map to break the firmware
into the constituent images.

Tested using a servo_micro firmware:
$ fwupdtool firmware-parse servo_micro_v2.4.17-df61092c3.bin
<select fmap option>
FuFmapFirmware:
  FuFirmwareImage:
  ID:                    EC_RO
  Index:                 0x1
  Data:                  0xf000
  FuFirmwareImage:
  ID:                    FR_MAIN
  Index:                 0x2
  Data:                  0xf000
  FuFirmwareImage:
  ID:                    RO_FRID
  Index:                 0x3
  Address:               0xc4
  Data:                  0x20
  FuFirmwareImage:
  ID:                    FMAP
  Index:                 0x4
  Address:               0x9a40
  Version:               1.0
  Data:                  0x15e
  FuFirmwareImage:
  ID:                    WP_RO
  Index:                 0x5
  Data:                  0x10000
  FuFirmwareImage:
  ID:                    EC_RW
  Index:                 0x6
  Address:               0x10000
  Data:                  0x10000
  FuFirmwareImage:
  ID:                    RW_FWID
  Index:                 0x7
  Address:               0x100c4
  Data:                  0x20
---
 libfwupdplugin/fu-fmap-firmware.c | 228 +++++++++++++++++++++++++++++-
 libfwupdplugin/fu-fmap-firmware.h |  38 ++++-
 2 files changed, 260 insertions(+), 6 deletions(-)

diff --git a/libfwupdplugin/fu-fmap-firmware.c b/libfwupdplugin/fu-fmap-firmware.c
index 8c3c46344..d41ac5f83 100644
--- a/libfwupdplugin/fu-fmap-firmware.c
+++ b/libfwupdplugin/fu-fmap-firmware.c
@@ -8,12 +8,160 @@
 
 #include "fu-fmap-firmware.h"
 
-struct _FuFmapFirmware {
-	FuFirmware		 parent_instance;
-};
+#define FMAP_SIGNATURE		"__FMAP__"
+#define FMAP_AREANAME		"FMAP"
 
 G_DEFINE_TYPE (FuFmapFirmware, fu_fmap_firmware, FU_TYPE_FIRMWARE)
 
+/* returns size of fmap data structure if successful, <0 to indicate error */
+static gint
+fmap_size (FuFmap *fmap)
+{
+	if (fmap == NULL)
+		return -1;
+
+	return sizeof (*fmap) + (fmap->nareas * sizeof (FuFmap));
+}
+
+/* brute force linear search */
+static gboolean
+fmap_lsearch (const guint8 *image, gsize len, gsize *offset, GError **error)
+{
+	gsize i;
+	gboolean fmap_found = FALSE;
+
+	if (offset == NULL) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "offset return not valid");
+		return FALSE;
+	}
+
+	for (i = 0; i < len - strlen(FMAP_SIGNATURE); i++) {
+		if (!memcmp(&image[i],
+		            FMAP_SIGNATURE,
+		            strlen(FMAP_SIGNATURE))) {
+			fmap_found = TRUE;
+			break;
+		}
+	}
+
+	if (!fmap_found) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "fmap not found using linear search");
+		return FALSE;
+	}
+
+	if (i + fmap_size ((FuFmap *)&image[i]) > len) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "malformed fmap too close to end of image");
+		return FALSE;
+	}
+
+	*offset = i;
+	return TRUE;
+}
+
+/* if image length is a power of 2, use binary search */
+static gboolean
+fmap_bsearch (const guint8 *image, gsize len, gsize *offset, GError **error)
+{
+	gsize i;
+	gboolean fmap_found = FALSE;
+
+	if (offset == NULL) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "offset return not valid");
+		return FALSE;
+	}
+
+	/*
+	 * For efficient operation, we start with the largest stride possible
+	 * and then decrease the stride on each iteration. Also, check for a
+	 * remainder when modding the offset with the previous stride. This
+	 * makes it so that each offset is only checked once.
+	 */
+	for (gint stride = len / 2; stride >= 1; stride /= 2) {
+		if (fmap_found)
+			break;
+
+		for (i = 0; i < len - strlen(FMAP_SIGNATURE); i += stride) {
+			if ((i % (stride * 2) == 0) && (i != 0))
+				continue;
+			if (!memcmp (&image[i],
+				     FMAP_SIGNATURE,
+				     strlen (FMAP_SIGNATURE))) {
+				fmap_found = TRUE;
+				break;
+			}
+		}
+	}
+
+	if (!fmap_found) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "fmap not found using binary search");
+		return FALSE;
+	}
+
+	if (i + fmap_size ((FuFmap *)&image[i]) > len) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "malformed fmap too close to end of image");
+		return FALSE;
+	}
+
+	*offset = i;
+	return TRUE;
+}
+
+static gint
+popcnt (guint u)
+{
+	gint count;
+
+	/* K&R method */
+	for (count = 0; u; count++)
+		u &= (u - 1);
+
+	return count;
+}
+
+static gboolean
+fmap_find (const guint8 *image, gsize image_len, gsize *offset, GError **error)
+{
+	if (image == NULL || image_len == 0) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "invalid image");
+		return FALSE;
+	}
+
+	if (popcnt (image_len) == 1) {
+		if (!fmap_bsearch (image, image_len, offset, error)) {
+			g_prefix_error (error, "failed fmap_find using bsearch: ");
+			return FALSE;
+		}
+	} else {
+		if (!fmap_lsearch (image, image_len, offset, error)) {
+			g_prefix_error (error, "failed fmap_find using lsearch: ");
+			return FALSE;
+		}
+	}
+
+	return TRUE;
+}
+
 static gboolean
 fu_fmap_firmware_parse (FuFirmware *firmware,
 			GBytes *fw,
@@ -22,8 +170,78 @@ fu_fmap_firmware_parse (FuFirmware *firmware,
 			FwupdInstallFlags flags,
 			GError **error)
 {
-	/* set bogus version */
-	fu_firmware_set_version (firmware, "1.2.3");
+	FuFmapFirmwareClass *klass_firmware = FU_FMAP_FIRMWARE_GET_CLASS (firmware);
+	gsize image_len;
+	guint8 *image = (guint8 *)g_bytes_get_data (fw, &image_len);
+	gsize offset;
+	const FuFmap *fmap;
+
+	/* corrupt */
+	if (g_bytes_get_size (fw) < sizeof (FuFmap)) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "firmware too small for fmap");
+		return FALSE;
+	}
+
+	if (!fmap_find (image, image_len, &offset, error)) {
+		g_set_error_literal (error,
+				     G_IO_ERROR,
+				     G_IO_ERROR_INVALID_DATA,
+				     "cannot find fmap in image");
+		return FALSE;
+	}
+
+	fmap = (const FuFmap *)(image + offset);
+
+	if (fmap->size != image_len) {
+		g_set_error (error,
+			     G_IO_ERROR,
+			     G_IO_ERROR_INVALID_DATA,
+			     "file size incorrect, expected 0x%04x got 0x%04x",
+			     (guint) fmap->size,
+			     (guint) image_len);
+		return FALSE;
+	}
+
+	if (fmap->nareas < 1) {
+		g_set_error (error,
+			     G_IO_ERROR,
+			     G_IO_ERROR_INVALID_DATA,
+			     "number of areas too small, got %" G_GUINT16_FORMAT,
+			     fmap->nareas);
+		return FALSE;
+	}
+
+	for (gsize i = 0; i < fmap->nareas; i++) {
+		const FuFmapArea *area = &fmap->areas[i];
+		g_autoptr(FuFirmwareImage) img = NULL;
+		g_autoptr(GBytes) bytes = NULL;
+
+		img = fu_firmware_image_new (NULL);
+		bytes = g_bytes_new_from_bytes (fw,
+						(gsize) area->offset,
+						(gsize) area->size);
+		fu_firmware_image_set_id (img, (const gchar *) area->name);
+		fu_firmware_image_set_idx (img, i + 1);
+		fu_firmware_image_set_addr (img, (guint64) area->offset);
+		fu_firmware_image_set_bytes (img, bytes);
+		fu_firmware_add_image (firmware, img);
+
+		if (g_strcmp0 ((const char *)area->name, FMAP_AREANAME) == 0) {
+			g_autofree gchar *version = g_strdup_printf ("%d.%d",
+								     fmap->ver_major,
+								     fmap->ver_minor);
+			fu_firmware_image_set_version (img, version);
+		}
+	}
+
+	/* subclassed */
+	if (klass_firmware->parse != NULL) {
+		if (!klass_firmware->parse (firmware, fw, addr_start, addr_end, flags, error))
+			return FALSE;
+	}
 
 	/* success */
 	return TRUE;
diff --git a/libfwupdplugin/fu-fmap-firmware.h b/libfwupdplugin/fu-fmap-firmware.h
index 934d826d4..70fb6b9c1 100644
--- a/libfwupdplugin/fu-fmap-firmware.h
+++ b/libfwupdplugin/fu-fmap-firmware.h
@@ -8,7 +8,43 @@
 
 #include "fu-firmware.h"
 
+#define FMAP_STRLEN		32	/* maximum length for strings, */
+					/* including null-terminator */
+
 #define FU_TYPE_FMAP_FIRMWARE (fu_fmap_firmware_get_type ())
-G_DECLARE_FINAL_TYPE (FuFmapFirmware, fu_fmap_firmware, FU, FMAP_FIRMWARE, FuFirmware)
+G_DECLARE_DERIVABLE_TYPE (FuFmapFirmware, fu_fmap_firmware, FU, FMAP_FIRMWARE, FuFirmware)
+
+struct _FuFmapFirmwareClass
+{
+	FuFirmwareClass		 parent_class;
+	gboolean		 (*parse)		(FuFirmware	*self,
+							 GBytes		*fw,
+							 guint64	 addr_start,
+							 guint64	 addr_end,
+							 FwupdInstallFlags flags,
+							 GError		**error);
+	/*< private >*/
+	gpointer		 padding[14];
+};
+
+/* mapping of volatile and static regions in firmware binary */
+typedef struct  __attribute__((packed)) {
+	guint32	offset;                /* offset relative to base */
+	guint32	size;                  /* size in bytes */
+	guint8	name[FMAP_STRLEN];     /* descriptive name */
+	guint16	flags;                 /* flags for this area */
+} FuFmapArea;
+
+typedef struct __attribute__((packed)) {
+	guint8		signature[8];		/* "__FMAP__" (0x5F5F464D41505F5F) */
+	guint8		ver_major;		/* major version */
+	guint8		ver_minor;		/* minor version */
+	guint64		base;			/* address of the firmware binary */
+	guint32		size;			/* size of firmware binary in bytes */
+	guint8		name[FMAP_STRLEN];	/* name of this firmware binary */
+	guint16		nareas;			/* number of areas described by
+						   areas[] below */
+	FuFmapArea	areas[];
+} FuFmap;
 
 FuFirmware			*fu_fmap_firmware_new			(void);