1 files changed, 96 insertions, 40 deletions

diff --git a/drivers/edgetpu/mobile-firmware.c b/drivers/edgetpu/mobile-firmware.c
index f021a32..88a2a4d 100644
--- a/drivers/edgetpu/mobile-firmware.c
+++ b/drivers/edgetpu/mobile-firmware.c
@@ -24,23 +24,38 @@
 #include "edgetpu-mobile-platform.h"
 #include "mobile-firmware.h"
 
+#define SSMT_NS_READ_STREAM_VID_OFFSET(n) (0x1000u + (0x4u * (n)))
+#define SSMT_NS_WRITE_STREAM_VID_OFFSET(n) (0x1200u + (0x4u * (n)))
+
+#define SSMT_NS_READ_STREAM_VID_REG(base, n)                                   \
+	((base) + SSMT_NS_READ_STREAM_VID_OFFSET(n))
+#define SSMT_NS_WRITE_STREAM_VID_REG(base, n)                                  \
+	((base) + SSMT_NS_WRITE_STREAM_VID_OFFSET(n))
+
 static struct mobile_image_config *mobile_firmware_get_image_config(struct edgetpu_dev *etdev)
 {
-	return (struct mobile_image_config *) edgetpu_firmware_get_data(etdev->firmware);
+	return edgetpu_firmware_get_data(etdev->firmware);
 }
 
-static void mobile_firmware_clear_mappings(struct edgetpu_dev *etdev,
-					   struct mobile_image_config *image_config)
+/* Clear mapping #i */
+static void clear_mapping(struct edgetpu_dev *etdev,
+			  struct mobile_image_config *image_config, int i)
 {
 	tpu_addr_t tpu_addr;
 	size_t size;
+
+	tpu_addr = image_config->mappings[i].virt_address;
+	size = CONFIG_TO_SIZE(image_config->mappings[i].image_config_value);
+	edgetpu_mmu_remove_translation(etdev, tpu_addr, size, EDGETPU_CONTEXT_KCI);
+}
+
+static void mobile_firmware_clear_mappings(struct edgetpu_dev *etdev,
+					   struct mobile_image_config *image_config)
+{
 	int i;
 
-	for (i = 0; i < image_config->num_iommu_mapping; i++) {
-		tpu_addr = image_config->mappings[i].virt_address;
-		size = CONFIG_TO_SIZE(image_config->mappings[i].image_config_value);
-		edgetpu_mmu_remove_translation(etdev, tpu_addr, size, EDGETPU_CONTEXT_KCI);
-	}
+	for (i = 0; i < image_config->num_iommu_mapping; i++)
+		clear_mapping(etdev, image_config, i);
 }
 
 static int mobile_firmware_setup_mappings(struct edgetpu_dev *etdev,
@@ -55,6 +70,7 @@ static int mobile_firmware_setup_mappings(struct edgetpu_dev *etdev,
 		tpu_addr = image_config->mappings[i].virt_address;
 		if (!tpu_addr) {
 			etdev_warn(etdev, "Invalid firmware header\n");
+			ret = -EIO;
 			goto err;
 		}
 		size = CONFIG_TO_SIZE(image_config->mappings[i].image_config_value);
@@ -76,26 +92,29 @@ static int mobile_firmware_setup_mappings(struct edgetpu_dev *etdev,
 	return 0;
 
 err:
-	while (i--) {
-		tpu_addr = image_config->mappings[i].virt_address;
-		size = CONFIG_TO_SIZE(image_config->mappings[i].image_config_value);
-		edgetpu_mmu_remove_translation(etdev, tpu_addr, size, EDGETPU_CONTEXT_KCI);
-	}
+	while (i--)
+		clear_mapping(etdev, image_config, i);
 	return ret;
 }
 
-static void mobile_firmware_clear_ns_mappings(struct edgetpu_dev *etdev,
-					      struct mobile_image_config *image_config)
+static void clear_ns_mapping(struct edgetpu_dev *etdev,
+			     struct mobile_image_config *image_config, int i)
 {
 	tpu_addr_t tpu_addr;
 	size_t size;
+
+	tpu_addr = image_config->ns_iommu_mappings[i] & ~(0xFFF);
+	size = CONFIG_TO_MBSIZE(image_config->ns_iommu_mappings[i]);
+	edgetpu_mmu_remove_translation(etdev, tpu_addr, size, EDGETPU_CONTEXT_KCI);
+}
+
+static void mobile_firmware_clear_ns_mappings(struct edgetpu_dev *etdev,
+					      struct mobile_image_config *image_config)
+{
 	int i;
 
-	for (i = 0; i < image_config->num_ns_iommu_mappings; i++) {
-		tpu_addr = image_config->ns_iommu_mappings[i] & ~(0xFFF);
-		size = CONFIG_TO_MBSIZE(image_config->ns_iommu_mappings[i]);
-		edgetpu_mmu_remove_translation(etdev, tpu_addr, size, EDGETPU_CONTEXT_KCI);
-	}
+	for (i = 0; i < image_config->num_ns_iommu_mappings; i++)
+		clear_ns_mapping(etdev, image_config, i);
 }
 
 static int mobile_firmware_setup_ns_mappings(struct edgetpu_dev *etdev,
@@ -129,11 +148,8 @@ static int mobile_firmware_setup_ns_mappings(struct edgetpu_dev *etdev,
 	return 0;
 
 err:
-	while (i--) {
-		tpu_addr = image_config->ns_iommu_mappings[i] & ~(0xFFF);
-		size = CONFIG_TO_MBSIZE(image_config->ns_iommu_mappings[i]);
-		edgetpu_mmu_remove_translation(etdev, tpu_addr, size, EDGETPU_CONTEXT_KCI);
-	}
+	while (i--)
+		clear_ns_mapping(etdev, image_config, i);
 	return ret;
 }
 
@@ -294,6 +310,43 @@ static void program_iremap_csr(struct edgetpu_dev *etdev)
 	edgetpu_dev_write_32(etdev, EDGETPU_REG_INSTRUCTION_REMAP_CONTROL + 8, 1);
 }
 
+static void mobile_firmware_setup_ssmt(struct edgetpu_dev *etdev)
+{
+	int i;
+	struct edgetpu_mobile_platform_dev *etmdev = to_mobile_dev(etdev);
+	struct mobile_image_config *image_config = mobile_firmware_get_image_config(etdev);
+
+	/*
+	 * This only works if the SSMT is set to client-driven mode, which only GSA can do.
+	 * Skip if GSA is not available
+	 */
+	if (!etmdev->ssmt_base || !etmdev->gsa_dev)
+		return;
+
+	etdev_dbg(etdev, "Setting up SSMT for privilege level: %d\n",
+		  image_config->privilege_level);
+
+	/*
+	 * Setup non-secure SCIDs, assume VID = SCID when running at TZ or GSA level,
+	 * Reset the table to zeroes if running non-secure firmware, since the SSMT
+	 * will be in clamped mode and we want all memory accesses to go to the
+	 * default page table.
+	 *
+	 * TODO(b/204384254) Confirm SSMT setup for Rio
+	 */
+	for (i = 0; i < EDGETPU_NCONTEXTS; i++) {
+		int val;
+
+		if (image_config->privilege_level == FW_PRIV_LEVEL_NS)
+			val = 0;
+		else
+			val = i;
+
+		writel(val, SSMT_NS_READ_STREAM_VID_REG(etmdev->ssmt_base, i));
+		writel(val, SSMT_NS_WRITE_STREAM_VID_REG(etmdev->ssmt_base, i));
+	}
+}
+
 static int mobile_firmware_prepare_run(struct edgetpu_firmware *et_fw,
 				       struct edgetpu_firmware_buffer *fw_buf)
 {
@@ -305,6 +358,8 @@ static int mobile_firmware_prepare_run(struct edgetpu_firmware *et_fw,
 	if (IS_ENABLED(CONFIG_RIO))
 		program_iremap_csr(etdev);
 
+	mobile_firmware_setup_ssmt(etdev);
+
 	return edgetpu_mobile_firmware_reset_cpu(etdev, false);
 }
 
@@ -385,23 +440,24 @@ static int mobile_firmware_setup_buffer(struct edgetpu_firmware *et_fw,
 		goto out;
 	}
 
-	if (image_config_changed) {
-		/* clear last image mappings */
-		if (last_image_config->privilege_level == FW_PRIV_LEVEL_NS)
-			mobile_firmware_clear_mappings(etdev, last_image_config);
+	if (!image_config_changed)
+		goto out;
 
-		if (image_config->privilege_level == FW_PRIV_LEVEL_NS)
-			ret = mobile_firmware_setup_mappings(etdev, image_config);
-		if (ret)
-			goto out;
-		mobile_firmware_clear_ns_mappings(etdev, last_image_config);
-		ret = mobile_firmware_setup_ns_mappings(etdev, image_config);
-		if (ret) {
-			mobile_firmware_clear_mappings(etdev, image_config);
-			goto out;
-		}
-		mobile_firmware_save_image_config(etdev, image_config);
+	/* clear last image mappings */
+	if (last_image_config->privilege_level == FW_PRIV_LEVEL_NS)
+		mobile_firmware_clear_mappings(etdev, last_image_config);
+
+	if (image_config->privilege_level == FW_PRIV_LEVEL_NS)
+		ret = mobile_firmware_setup_mappings(etdev, image_config);
+	if (ret)
+		goto out;
+	mobile_firmware_clear_ns_mappings(etdev, last_image_config);
+	ret = mobile_firmware_setup_ns_mappings(etdev, image_config);
+	if (ret) {
+		mobile_firmware_clear_mappings(etdev, image_config);
+		goto out;
 	}
+	mobile_firmware_save_image_config(etdev, image_config);
 out:
 	memunmap(image_vaddr);
 	return ret;