ARM: stm32mp: fix RIFSC semaphores acquition

Fix RIFSC semaphores acquisition by not returning an error when the
current CID already possess the semaphore. Also fix an incorrect mask
for the CID value in the SEMCR register.

Signed-off-by: Gatien Chevallier <gatien.chevallier@foss.st.com>
Change-Id: I9900d4307f24a5b720a9a38846f197dbb5205987
Reviewed-on: https://gerrit.st.com/c/mpu/oe/st/u-boot/+/440109
Reviewed-by: Patrice CHOTARD <patrice.chotard@foss.st.com>
Domain-Review: Yann GAUTIER <yann.gautier@foss.st.com>
Tested-by: Gatien CHEVALLIER <gatien.chevallier@st.com>
Reviewed-by: Gatien CHEVALLIER <gatien.chevallier@st.com>
ACI: CITOOLS <MDG-smet-aci-reviews@list.st.com>
ACI: CIBUILD <MDG-smet-aci-builds@list.st.com>

Author: GseoC

arch/arm/mach-stm32mp/stm32mp2/rifsc.c

@@ -74,7 +74,8 @@ static int stm32_rif_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(base, id) &&
+	    FIELD_GET(RIFSC_RISC_SCID_MASK, (readl(addr)) != RIF_CID1))
 		return -EACCES;
 
 	setbits_le32(addr, SEMCR_MUTEX);
@@ -161,7 +162,7 @@ static int rifsc_check_access(void *base, u32 id)
 			return -EACCES;
 		}
 		if (!stm32_rif_is_semaphore_available(base, id) &&
-		    !(FIELD_GET(RIFSC_RISC_SCID_MASK, sem_reg_value) & BIT(RIF_CID1))) {
+		    !(FIELD_GET(RIFSC_RISC_SCID_MASK, sem_reg_value) & RIF_CID1)) {
 			log_debug("Semaphore unavailable for peripheral %d\n", id);
 			return -EACCES;
 		}