1 files changed, 76 insertions, 54 deletions

diff --git a/arch/arm/mach-stm32mp/stm32mp2/rifsc.c b/arch/arm/mach-stm32mp/stm32mp2/rifsc.c
index 50dececf77b..f8f67af4449 100644
--- a/arch/arm/mach-stm32mp/stm32mp2/rifsc.c
+++ b/arch/arm/mach-stm32mp/stm32mp2/rifsc.c
@@ -61,42 +61,41 @@ struct stm32_rifsc_child_plat {
 	u32 domain_id;
 };
 
-static bool stm32_rif_is_semaphore_available(void *base, u32 id)
+static bool stm32_rif_is_semaphore_available(void *addr)
 {
-	void *addr = base + RIFSC_RISC_PER0_SEMCR(id);
-
 	return !(readl(addr) & SEMCR_MUTEX);
 }
 
-static int stm32_rif_acquire_semaphore(void *base, u32 id)
+static int stm32_rifsc_acquire_semaphore(void *base, u32 id)
 {
 	void *addr = base + RIFSC_RISC_PER0_SEMCR(id);
 
 	/* Check that the semaphore is available */
-	if (!stm32_rif_is_semaphore_available(base, id))
+	if (!stm32_rif_is_semaphore_available(addr) &&
+	    FIELD_GET(RIFSC_RISC_SCID_MASK, (readl(addr)) != RIF_CID1))
 		return -EACCES;
 
 	setbits_le32(addr, SEMCR_MUTEX);
 
 	/* Check that CID1 has the semaphore */
-	if (stm32_rif_is_semaphore_available(base, id) ||
+	if (stm32_rif_is_semaphore_available(addr) ||
 	    FIELD_GET(RIFSC_RISC_SCID_MASK, (readl(addr)) != RIF_CID1))
 		return -EACCES;
 
 	return 0;
 }
 
-static int stm32_rif_release_semaphore(void *base, u32 id)
+static int stm32_rifsc_release_semaphore(void *base, u32 id)
 {
 	void *addr = base + RIFSC_RISC_PER0_SEMCR(id);
 
-	if (stm32_rif_is_semaphore_available(base, id))
+	if (stm32_rif_is_semaphore_available(addr))
 		return 0;
 
 	clrbits_le32(addr, SEMCR_MUTEX);
 
 	/* Ok if another compartment takes the semaphore before the check */
-	if (!stm32_rif_is_semaphore_available(base, id) &&
+	if (!stm32_rif_is_semaphore_available(addr) &&
 	    FIELD_GET(RIFSC_RISC_SCID_MASK, (readl(addr)) == RIF_CID1))
 		return -EACCES;
 
@@ -105,11 +104,10 @@ static int stm32_rif_release_semaphore(void *base, u32 id)
 
 static int rifsc_parse_access_controller(ofnode node, struct ofnode_phandle_args *args)
 {
-	int ret;
+	int ret = ofnode_parse_phandle_with_args(node, "access-controllers",
+						 "#access-controller-cells", 0,
+						 0, args);
 
-	ret = ofnode_parse_phandle_with_args(node, "access-controllers",
-					     "#access-controller-cells", 0,
-					     0, args);
 	if (ret) {
 		log_debug("failed to parse access-controller (%d)\n", ret);
 		return ret;
@@ -170,8 +168,8 @@ static int rifsc_check_access(void *base, u32 id)
 			log_debug("Not in semaphore whitelist for peripheral %d\n", id);
 			return -EACCES;
 		}
-		if (!stm32_rif_is_semaphore_available(base, id) &&
-		    !(FIELD_GET(RIFSC_RISC_SCID_MASK, sem_reg_value) & BIT(RIF_CID1))) {
+		if (!stm32_rif_is_semaphore_available(base + RIFSC_RISC_PER0_SEMCR(id)) &&
+		    !(FIELD_GET(RIFSC_RISC_SCID_MASK, sem_reg_value) & RIF_CID1)) {
 			log_debug("Semaphore unavailable for peripheral %d\n", id);
 			return -EACCES;
 		}
@@ -187,22 +185,44 @@ skip_cid_check:
 	return 0;
 }
 
-int stm32_rifsc_check_access_by_id(ofnode device_node, u32 id)
+int stm32_rifsc_grant_access_by_id(ofnode device_node, u32 id)
 {
 	struct ofnode_phandle_args args;
+	u32 cid_reg_value;
+	void *rifsc_base;
 	int err;
 
-	if (id >= STM32MP25_RIFSC_ENTRIES)
-		return -EINVAL;
-
 	err = rifsc_parse_access_controller(device_node, &args);
 	if (err)
+		panic("Failed to parse access-controllers property\n");
+
+	rifsc_base = (void *)ofnode_get_addr(args.node);
+
+	err = rifsc_check_access(rifsc_base, id);
+	if (err)
 		return err;
 
-	return rifsc_check_access((void *)ofnode_get_addr(args.node), id);
+	cid_reg_value = readl(rifsc_base + RIFSC_RISC_PER0_CIDCFGR(id));
+
+	/*
+	 * If the peripheral is in semaphore mode, take the semaphore so that
+	 * the CID1 has the ownership.
+	 */
+	if (cid_reg_value & CIDCFGR_SEMEN &&
+	    (FIELD_GET(RIFSC_RISC_SEMWL_MASK, cid_reg_value) & BIT(RIF_CID1))) {
+		err = stm32_rifsc_acquire_semaphore(rifsc_base, id);
+		if (err) {
+			pr_err("Couldn't acquire RIF semaphore for peripheral %d (%d)\n",
+			       id, err);
+			return err;
+		}
+		pr_debug("Acquiring RIF semaphore for peripheral %d\n", id);
+	}
+
+	return 0;
 }
 
-int stm32_rifsc_check_access(ofnode device_node)
+int stm32_rifsc_grant_access(ofnode device_node)
 {
 	struct ofnode_phandle_args args;
 	int err;
@@ -211,58 +231,60 @@ int stm32_rifsc_check_access(ofnode device_node)
 	if (err)
 		return err;
 
-	return rifsc_check_access((void *)ofnode_get_addr(args.node), args.args[0]);
+	return stm32_rifsc_grant_access_by_id(device_node, args.args[0]);
+
 }
 
-static int stm32_rifsc_child_pre_probe(struct udevice *dev)
+void stm32_rifsc_release_access_by_id(ofnode device_node, u32 id)
 {
-	struct stm32_rifsc_plat *plat = dev_get_plat(dev->parent);
-	struct stm32_rifsc_child_plat *child_plat = dev_get_parent_plat(dev);
+	struct ofnode_phandle_args args;
 	u32 cid_reg_value;
+	void *rifsc_base;
 	int err;
-	u32 id = child_plat->domain_id;
 
-	cid_reg_value = readl(plat->base + RIFSC_RISC_PER0_CIDCFGR(id));
+	err = rifsc_parse_access_controller(device_node, &args);
+	if (err)
+		panic("Failed to parse access-controllers property\n");
 
-	/*
-	 * If the peripheral is in semaphore mode, take the semaphore so that
-	 * the CID1 has the ownership.
-	 */
+	rifsc_base = (void *)ofnode_get_addr(args.node);
+
+	cid_reg_value = readl(rifsc_base + RIFSC_RISC_PER0_CIDCFGR(id));
+
+	/* If the peripheral is in semaphore mode, release it if we have the ownership */
 	if (cid_reg_value & CIDCFGR_SEMEN &&
 	    (FIELD_GET(RIFSC_RISC_SEMWL_MASK, cid_reg_value) & BIT(RIF_CID1))) {
-		err = stm32_rif_acquire_semaphore(plat->base, id);
+		err = stm32_rifsc_release_semaphore(rifsc_base, id);
 		if (err) {
-			dev_err(dev, "Couldn't acquire RIF semaphore for peripheral %d (%d)\n",
-				id, err);
-			return err;
+			panic("Couldn't release RIF semaphore for peripheral %d (%d)\n", id, err);
 		}
-		dev_dbg(dev, "Acquiring semaphore for peripheral %d\n", id);
+		pr_debug("Releasing RIF semaphore for peripheral %d\n", id);
 	}
+}
 
-	return 0;
+void stm32_rifsc_release_access(ofnode device_node)
+{
+	struct ofnode_phandle_args args;
+	int err;
+
+	err = rifsc_parse_access_controller(device_node, &args);
+	if (err)
+		panic("Failed to parse access-controllers property\n");
+
+	stm32_rifsc_release_access_by_id(device_node, args.args[0]);
 }
 
-static int stm32_rifsc_child_post_remove(struct udevice *dev)
+static int stm32_rifsc_child_pre_probe(struct udevice *dev)
 {
-	struct stm32_rifsc_plat *plat = dev_get_plat(dev->parent);
 	struct stm32_rifsc_child_plat *child_plat = dev_get_parent_plat(dev);
-	u32 cid_reg_value;
-	int err;
-	u32 id = child_plat->domain_id;
 
-	cid_reg_value = readl(plat->base + RIFSC_RISC_PER0_CIDCFGR(id));
+	return stm32_rifsc_grant_access_by_id(dev_ofnode(dev), child_plat->domain_id);
+}
 
-	/*
-	 * If the peripheral is in semaphore mode, release the semaphore so that
-	 * there's no ownership.
-	 */
-	if (cid_reg_value & CIDCFGR_SEMEN &&
-	    (FIELD_GET(RIFSC_RISC_SEMWL_MASK, cid_reg_value) & BIT(RIF_CID1))) {
-		err = stm32_rif_release_semaphore(plat->base, id);
-		if (err)
-			dev_err(dev, "Couldn't release rif semaphore for peripheral %d (%d)\n",
-				id, err);
-	}
+static int stm32_rifsc_child_post_remove(struct udevice *dev)
+{
+	struct stm32_rifsc_child_plat *child_plat = dev_get_parent_plat(dev);
+
+	stm32_rifsc_release_access_by_id(dev_ofnode(dev), child_plat->domain_id);
 
 	return 0;
 }