1 files changed, 220 insertions, 0 deletions

diff --git a/gcip-kernel-driver/drivers/gcip/gcip-image-config.c b/gcip-kernel-driver/drivers/gcip/gcip-image-config.c
new file mode 100644
index 0000000..312bbdc
--- /dev/null
+++ b/gcip-kernel-driver/drivers/gcip/gcip-image-config.c
@@ -0,0 +1,220 @@
+// SPDX-License-Identifier: GPL-2.0
+/*
+ * Framework for parsing the firmware image configuration.
+ *
+ * Copyright (C) 2022 Google LLC
+ */
+
+#include <linux/device.h>
+#include <linux/errno.h>
+#include <linux/string.h>
+#include <linux/types.h>
+
+#include <gcip/gcip-image-config.h>
+
+#define ADDR_SHIFT 12
+#define SIZE_MODE_BIT BIT(ADDR_SHIFT - 1)
+#define SECURE_SIZE_MASK (SIZE_MODE_BIT - 1u)
+#define NS_SIZE_MASK (BIT(ADDR_SHIFT) - 1u)
+#define ADDR_MASK ~(BIT(ADDR_SHIFT) - 1u)
+
+/* used by ns_iommu_mappings */
+#define CONFIG_TO_MBSIZE(a) (((a) & NS_SIZE_MASK) << 20)
+
+/* used by iommu_mappings */
+static inline __u32 config_to_size(__u32 cfg)
+{
+	__u32 page_size;
+
+	if (cfg & SIZE_MODE_BIT)
+		page_size = cfg & SECURE_SIZE_MASK;
+	else
+		page_size = BIT(cfg & SECURE_SIZE_MASK);
+
+	return page_size << PAGE_SHIFT;
+}
+
+static int setup_iommu_mappings(struct gcip_image_config_parser *parser,
+				struct gcip_image_config *config)
+{
+	int i, ret;
+	dma_addr_t daddr;
+	size_t size;
+	phys_addr_t paddr;
+
+	for (i = 0; i < config->num_iommu_mappings; i++) {
+		daddr = config->iommu_mappings[i].virt_address;
+		if (unlikely(!daddr)) {
+			dev_warn(parser->dev, "Invalid config, device address is zero");
+			ret = -EIO;
+			goto err;
+		}
+		size = config_to_size(config->iommu_mappings[i].image_config_value);
+		paddr = config->iommu_mappings[i].image_config_value & ADDR_MASK;
+
+		dev_dbg(parser->dev, "Image config adding IOMMU mapping: %pad -> %pap", &daddr,
+			&paddr);
+
+		if (unlikely(daddr + size <= daddr || paddr + size <= paddr)) {
+			ret = -EOVERFLOW;
+			goto err;
+		}
+		ret = parser->ops->map(parser->data, daddr, paddr, size,
+				       GCIP_IMAGE_CONFIG_FLAGS_SECURE);
+		if (ret) {
+			dev_err(parser->dev,
+				"Unable to Map: %d dma_addr: %pad phys_addr: %pap size: %#lx\n",
+				ret, &daddr, &paddr, size);
+			goto err;
+		}
+	}
+
+	return 0;
+
+err:
+	while (i--) {
+		daddr = config->iommu_mappings[i].virt_address;
+		size = config_to_size(config->iommu_mappings[i].image_config_value);
+		parser->ops->unmap(parser->data, daddr, size, GCIP_IMAGE_CONFIG_FLAGS_SECURE);
+	}
+	return ret;
+}
+
+static void clear_iommu_mappings(struct gcip_image_config_parser *parser,
+				 struct gcip_image_config *config)
+{
+	dma_addr_t daddr;
+	size_t size;
+	int i;
+
+	for (i = config->num_iommu_mappings - 1; i >= 0; i--) {
+		daddr = config->iommu_mappings[i].virt_address;
+		size = config_to_size(config->iommu_mappings[i].image_config_value);
+		dev_dbg(parser->dev, "Image config removing IOMMU mapping: %pad size=%#lx", &daddr,
+			size);
+		parser->ops->unmap(parser->data, daddr, size, GCIP_IMAGE_CONFIG_FLAGS_SECURE);
+	}
+}
+
+static int setup_ns_iommu_mappings(struct gcip_image_config_parser *parser,
+				   struct gcip_image_config *config)
+{
+	dma_addr_t daddr;
+	size_t size;
+	int ret, i;
+	phys_addr_t paddr = 0;
+
+	for (i = 0; i < config->num_ns_iommu_mappings; i++) {
+		daddr = config->ns_iommu_mappings[i] & ADDR_MASK;
+		if (unlikely(!daddr)) {
+			dev_warn(parser->dev, "Invalid config, device address is zero");
+			ret = -EIO;
+			goto err;
+		}
+		size = CONFIG_TO_MBSIZE(config->ns_iommu_mappings[i]);
+		dev_dbg(parser->dev, "Image config adding NS IOMMU mapping: %pad -> %pap", &daddr,
+			&paddr);
+		if (unlikely(daddr + size <= daddr || paddr + size <= paddr)) {
+			ret = -EOVERFLOW;
+			goto err;
+		}
+		ret = parser->ops->map(parser->data, daddr, paddr, size, 0);
+		if (ret)
+			goto err;
+		paddr += size;
+	}
+
+	return 0;
+
+err:
+	while (i--) {
+		size = CONFIG_TO_MBSIZE(config->ns_iommu_mappings[i]);
+		daddr = config->ns_iommu_mappings[i] & ADDR_MASK;
+		parser->ops->unmap(parser->data, daddr, size, 0);
+	}
+	return ret;
+}
+
+static void clear_ns_iommu_mappings(struct gcip_image_config_parser *parser,
+				    struct gcip_image_config *config)
+{
+	dma_addr_t daddr;
+	size_t size;
+	int i;
+
+	for (i = config->num_ns_iommu_mappings - 1; i >= 0; i--) {
+		size = CONFIG_TO_MBSIZE(config->ns_iommu_mappings[i]);
+		daddr = config->ns_iommu_mappings[i] & ADDR_MASK;
+		dev_dbg(parser->dev, "Image config removing NS IOMMU mapping: %pad size=%#lx",
+			&daddr, size);
+		parser->ops->unmap(parser->data, daddr, size, 0);
+	}
+}
+
+static int map_image_config(struct gcip_image_config_parser *parser,
+			    struct gcip_image_config *config)
+{
+	int ret = setup_ns_iommu_mappings(parser, config);
+
+	if (ret)
+		return ret;
+	if (gcip_image_config_is_ns(config)) {
+		ret = setup_iommu_mappings(parser, config);
+		if (ret)
+			clear_ns_iommu_mappings(parser, config);
+	}
+	return ret;
+}
+
+static void unmap_image_config(struct gcip_image_config_parser *parser,
+			       struct gcip_image_config *config)
+{
+	if (gcip_image_config_is_ns(config))
+		clear_iommu_mappings(parser, config);
+	clear_ns_iommu_mappings(parser, config);
+}
+
+int gcip_image_config_parser_init(struct gcip_image_config_parser *parser,
+				  const struct gcip_image_config_ops *ops, struct device *dev,
+				  void *data)
+{
+	if (!ops->map || !ops->unmap) {
+		dev_err(dev, "Missing mandatory operations for image config parser");
+		return -EINVAL;
+	}
+	parser->dev = dev;
+	parser->data = data;
+	parser->ops = ops;
+	memset(&parser->last_config, 0, sizeof(parser->last_config));
+	return 0;
+}
+
+int gcip_image_config_parse(struct gcip_image_config_parser *parser,
+			    struct gcip_image_config *config)
+{
+	int ret;
+
+	if (!memcmp(config, &parser->last_config, sizeof(*config)))
+		return 0;
+	unmap_image_config(parser, &parser->last_config);
+	ret = map_image_config(parser, config);
+	if (ret) {
+		dev_err(parser->dev, "Map image config failed: %d", ret);
+		/*
+		 * Weird case as the mappings in the last config were just removed - might happen
+		 * if the IOMMU driver state is corrupted. We can't help to rescue it so let's
+		 * simply log a message.
+		 */
+		if (unlikely(map_image_config(parser, &parser->last_config)))
+			dev_err(parser->dev, "Failed to roll back the last image config");
+		return ret;
+	}
+	memcpy(&parser->last_config, config, sizeof(parser->last_config));
+	return 0;
+}
+
+void gcip_image_config_clear(struct gcip_image_config_parser *parser)
+{
+	unmap_image_config(parser, &parser->last_config);
+	memset(&parser->last_config, 0, sizeof(parser->last_config));
+}