From bd3f60d29c2494d156b353c33678be7366caafbd Mon Sep 17 00:00:00 2001
From: Patrick Delaunay <patrick.delaunay@st.com>
Date: Tue, 16 Jun 2020 18:27:44 +0200
Subject: [PATCH] arm: stm32mp: protect DBGMCU_IDC access with BSEC

As debugger must be totally closed on Sec closed chip,
the DBGMCU_IDC register is no more accessible (self
hosted debug is disabled with OTP).

This patch adds a function bsec_dbgswenable() to check
if the DBGMCU registers are available before to access them:
BSEC_DENABLE.DBGSWENABLE = self hosted debug status.

Signed-off-by: Patrick Delaunay <patrick.delaunay@st.com>
Reviewed-by: Patrice Chotard <patrice.chotard@st.com>
---
 arch/arm/mach-stm32mp/bsec.c              | 25 +++++++++++++++++++++++
 arch/arm/mach-stm32mp/cpu.c               | 22 ++++++++++++++++----
 arch/arm/mach-stm32mp/include/mach/bsec.h |  7 +++++++
 3 files changed, 50 insertions(+), 4 deletions(-)
 create mode 100644 arch/arm/mach-stm32mp/include/mach/bsec.h

diff --git a/arch/arm/mach-stm32mp/bsec.c b/arch/arm/mach-stm32mp/bsec.c
index 059ef0b1f5..0c56b440f5 100644
--- a/arch/arm/mach-stm32mp/bsec.c
+++ b/arch/arm/mach-stm32mp/bsec.c
@@ -8,6 +8,7 @@
 #include <log.h>
 #include <misc.h>
 #include <asm/io.h>
+#include <asm/arch/bsec.h>
 #include <asm/arch/stm32mp1_smc.h>
 #include <linux/arm-smccc.h>
 #include <linux/iopoll.h>
@@ -21,6 +22,7 @@
 #define BSEC_OTP_WRDATA_OFF		0x008
 #define BSEC_OTP_STATUS_OFF		0x00C
 #define BSEC_OTP_LOCK_OFF		0x010
+#define BSEC_DENABLE_OFF		0x014
 #define BSEC_DISTURBED_OFF		0x01C
 #define BSEC_ERROR_OFF			0x034
 #define BSEC_WRLOCK_OFF			0x04C /* OTP write permananet lock */
@@ -46,6 +48,9 @@
 #define BSEC_MODE_PROGFAIL_MASK		0x10
 #define BSEC_MODE_PWR_MASK		0x20
 
+/* DENABLE Register */
+#define BSEC_DENABLE_DBGSWENABLE	BIT(10)
+
 /*
  * OTP Lock services definition
  * Value must corresponding to the bit number in the register
@@ -506,3 +511,23 @@ U_BOOT_DRIVER(stm32mp_bsec) = {
 	.ops = &stm32mp_bsec_ops,
 	.probe = stm32mp_bsec_probe,
 };
+
+bool bsec_dbgswenable(void)
+{
+	struct udevice *dev;
+	struct stm32mp_bsec_platdata *plat;
+	int ret;
+
+	ret = uclass_get_device_by_driver(UCLASS_MISC,
+					  DM_GET_DRIVER(stm32mp_bsec), &dev);
+	if (ret || !dev) {
+		pr_debug("bsec driver not available\n");
+		return false;
+	}
+
+	plat = dev_get_platdata(dev);
+	if (readl(plat->base + BSEC_DENABLE_OFF) & BSEC_DENABLE_DBGSWENABLE)
+		return true;
+
+	return false;
+}
diff --git a/arch/arm/mach-stm32mp/cpu.c b/arch/arm/mach-stm32mp/cpu.c
index 472b140321..382067190c 100644
--- a/arch/arm/mach-stm32mp/cpu.c
+++ b/arch/arm/mach-stm32mp/cpu.c
@@ -12,6 +12,7 @@
 #include <misc.h>
 #include <net.h>
 #include <asm/io.h>
+#include <asm/arch/bsec.h>
 #include <asm/arch/stm32.h>
 #include <asm/arch/sys_proto.h>
 #include <dm/device.h>
@@ -155,8 +156,13 @@ static void dbgmcu_init(void)
 {
 	setbits_le32(RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
 
-	/* Freeze IWDG2 if Cortex-A7 is in debug mode */
-	setbits_le32(DBGMCU_APB4FZ1, DBGMCU_APB4FZ1_IWDG2);
+	/*
+	 * Freeze IWDG2 if Cortex-A7 is in debug mode
+	 * done in TF-A for TRUSTED boot and
+	 * DBGMCU access is controlled by BSEC_DENABLE.DBGSWENABLE
+	*/
+	if (!IS_ENABLED(CONFIG_TFABOOT) && bsec_dbgswenable())
+		setbits_le32(DBGMCU_APB4FZ1, DBGMCU_APB4FZ1_IWDG2);
 }
 #endif /* !defined(CONFIG_SPL) || defined(CONFIG_SPL_BUILD) */
 
@@ -276,9 +282,17 @@ void enable_caches(void)
 
 static u32 read_idc(void)
 {
-	setbits_le32(RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
+	/* DBGMCU access is controlled by BSEC_DENABLE.DBGSWENABLE */
+	if (bsec_dbgswenable()) {
+		setbits_le32(RCC_DBGCFGR, RCC_DBGCFGR_DBGCKEN);
 
-	return readl(DBGMCU_IDC);
+		return readl(DBGMCU_IDC);
+	}
+
+	if (CONFIG_IS_ENABLED(STM32MP15x))
+		return CPU_DEV_STM32MP15; /* STM32MP15x and unknown revision */
+	else
+		return 0x0;
 }
 
 u32 get_cpu_dev(void)
diff --git a/arch/arm/mach-stm32mp/include/mach/bsec.h b/arch/arm/mach-stm32mp/include/mach/bsec.h
new file mode 100644
index 0000000000..252eac3946
--- /dev/null
+++ b/arch/arm/mach-stm32mp/include/mach/bsec.h
@@ -0,0 +1,7 @@
+/* SPDX-License-Identifier: GPL-2.0+ OR BSD-3-Clause */
+/*
+ * Copyright (C) 2020, STMicroelectronics - All Rights Reserved
+ */
+
+/* check self hosted debug status = BSEC_DENABLE.DBGSWENABLE */
+bool bsec_dbgswenable(void);
-- 
GitLab