[PATCH v4 0/6] arm64: Add support for FEAT_Debugv8p9

[PATCH v4 0/6] arm64: Add support for FEAT_Debugv8p9

[Rob Herring (Arm)]

This series enables FEAT_Debugv8p9 which extends the breakpoint and 
watchpoint support from 16 up to 64. I've picked up this series from 
Anshuman.

Changes in V4:

- Limit enabling of FEAT_Debugv8p9 to only when more than 16 breakpoints 
  or watchpoints are present.
- Add lockdep_assert_irqs_disabled() to document the constraints. Drop 
  unnecessary IRQ disabling.
- Add more kprobe blacklist annotations and disallow breakpoints in the 
  nokprobe code.
- Drop previously applied sysreg patches.

Prior versions from Anshuman:

Changes in V3:

https://lore.kernel.org/all/20241216040831.2448257-1-anshuman.khandual@arm.com/

- Marked ID_AA64DFR1_EL1.ABLE as FTR_NONSTRICT in ftr_id_aa64dfr1[]
- Dropped MDCR_EL3.TDA boot requirement from documentation (separate series)
- Dropped MDCR_EL2_EBWE definition as MDCR_EL2 is now defined in tools sysreg
- Used SYS_FIELD_PREP() in read_wb_reg() and write_wb_reg()
- Added MAX_PER_BANK based BUILD_BUG_ON() tests in arch_hw_breakpoint_init()
- Dropped local variables i.e mdsel_bank and index
- Derived bank and index from MAX_PER_BANK as required

Changes in V2:

https://lore.kernel.org/all/20241028053426.2486633-1-anshuman.khandual@arm.com/

Following changes have been made per review comments from Mark Rutland

- Orr MDCR_EL2_EBWE directly without an intermittent register
- Alphabetically order header files in debug-monitors.c
- Dropped embwe_ref_count mechanism
- Dropped preempt_enable() from AARCH64_DBG_READ
- Dropped preempt_disable() from AARCH64_DBG_WRITE
- Dropped set_bank_index()
- Renamed read/write_wb_reg() as __read/__write_wb_reg()
- Modified read/write_wb_reg() to have MDSELR_E1 based banked read/write
- Added required sysreg tools patches from KVM FEAT_FGT2 series for build

Changes in V1:

https://lore.kernel.org/all/20241001043602.1116991-1-anshuman.khandual@arm.com/

- Changed FTR_STRICT to FTR_NONSTRICT for the following ID_AA64DFR1_EL1
  register fields - ABL_CMPs, DPFZS, PMICNTR, CTX_CMPs, WRPs and BRPs

Changes in RFC V2:

https://lore.kernel.org/linux-arm-kernel/20240620092607.267132-1-anshuman.khandual@arm.com/

- This series has been split from RFC V1 dealing only with arm64 breakpoints
- Restored back DBG_MDSCR_MASK definition (unrelated change)
- Added preempt_disable()/enable() blocks between selecting banks and registers

Changes in RFC:

https://lore.kernel.org/all/20240405080008.1225223-1-anshuman.khandual@arm.com/

Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
Anshuman Khandual (3):
      arm64/cpufeature: Add field details for ID_AA64DFR1_EL1 register
      arm64/boot: Enable EL2 requirements for FEAT_Debugv8p9
      arm64: hw_breakpoint: Enable FEAT_Debugv8p9

Rob Herring (Arm) (3):
      arm64: hw_breakpoint: Disallow breakpoints in no kprobe code
      arm64: hw_breakpoint: Add additional kprobe excluded functions
      arm64: hw_breakpoint: Add lockdep_assert_irqs_disabled() on install/uninstall

 Documentation/arch/arm64/booting.rst   | 13 +++++++
 arch/arm64/include/asm/el2_setup.h     | 14 +++++++
 arch/arm64/include/asm/hw_breakpoint.h | 47 ++++++++++++++++++-----
 arch/arm64/kernel/cpufeature.c         | 21 ++++++++---
 arch/arm64/kernel/debug-monitors.c     | 16 +++++---
 arch/arm64/kernel/hw_breakpoint.c      | 68 +++++++++++++++++++++++++++++-----
 6 files changed, 150 insertions(+), 29 deletions(-)
---
base-commit: 6de23f81a5e08be8fbf5e8d7e9febc72a5b5f27f
change-id: 20260406-arm-debug-8-9-41f6f0e739b5

Best regards,
--  
Rob Herring (Arm) <robh@kernel.org>

[PATCH v4 1/6] arm64: hw_breakpoint: Disallow breakpoints in no kprobe code

[Rob Herring (Arm)]

Taking debug exceptions while manipulating the breakpoints is likely to
be unsafe. The setting kprobes in the breakpoint code is already
forbidden, but the setting of h/w breakpoints is not. Copy what x86 does
and exclude breakpoints that fall within the kprobe section.

Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
 arch/arm64/kernel/hw_breakpoint.c | 10 ++++++++++
 1 file changed, 10 insertions(+)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index ab76b36dce82..38fbd67b2a6e 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -418,6 +418,16 @@ static int arch_build_bp_info(struct perf_event *bp,
 	/* Type */
 	switch (attr->bp_type) {
 	case HW_BREAKPOINT_X:
+		/*
+		 * We don't allow kernel breakpoints in places that are not
+		 * acceptable for kprobes.  On non-kprobes kernels, we don't
+		 * allow kernel breakpoints at all.
+		 */
+		if (attr->bp_addr >= TASK_SIZE_MAX) {
+			if (within_kprobe_blacklist(attr->bp_addr))
+				return -EINVAL;
+		}
+
 		hw->ctrl.type = ARM_BREAKPOINT_EXECUTE;
 		break;
 	case HW_BREAKPOINT_R:

-- 
2.53.0

[PATCH v4 2/6] arm64: hw_breakpoint: Add additional kprobe excluded functions

[Rob Herring (Arm)]

Everything that either runs during exceptions or touches the
breakpoint/watchpoint registers should be excluded from kprobes and
breakpoints.

The static functions are may or may not end up in the no kprobe section
depending on whether the compiler inlines them or not. They are likely
inlined, but make it explicit to ensure that they always are.
Unfortunately, it is not possible to leave the inlining decision up to
the compiler and place code within the no kprobes section.

Parts of what hw_breakpoint_control() calls are excluded already. Just
exclude all of it to be safe.

Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
 arch/arm64/kernel/hw_breakpoint.c | 15 ++++++++-------
 1 file changed, 8 insertions(+), 7 deletions(-)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index 38fbd67b2a6e..bb39bc759810 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -187,9 +187,9 @@ static int is_compat_bp(struct perf_event *bp)
  *	-ENOSPC if no slot is available/matches
  *	-EINVAL on wrong operations parameter
  */
-static int hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
-				    struct perf_event *bp,
-				    enum hw_breakpoint_ops ops)
+static nokprobe_inline int
+hw_breakpoint_slot_setup(struct perf_event **slots, int max_slots,
+			 struct perf_event *bp, enum hw_breakpoint_ops ops)
 {
 	int i;
 	struct perf_event **slot;
@@ -283,6 +283,7 @@ static int hw_breakpoint_control(struct perf_event *bp,
 
 	return 0;
 }
+NOKPROBE_SYMBOL(hw_breakpoint_control);
 
 /*
  * Install a perf counter breakpoint.
@@ -718,8 +719,8 @@ NOKPROBE_SYMBOL(do_breakpoint);
  * The function returns the distance of the address from the bytes watched by
  * the watchpoint. In case of an exact match, it returns 0.
  */
-static u64 get_distance_from_watchpoint(unsigned long addr, u64 val,
-					struct arch_hw_breakpoint_ctrl *ctrl)
+static nokprobe_inline u64 get_distance_from_watchpoint(unsigned long addr, u64 val,
+							struct arch_hw_breakpoint_ctrl *ctrl)
 {
 	u64 wp_low, wp_high;
 	u32 lens, lene;
@@ -739,8 +740,8 @@ static u64 get_distance_from_watchpoint(unsigned long addr, u64 val,
 		return 0;
 }
 
-static int watchpoint_report(struct perf_event *wp, unsigned long addr,
-			     struct pt_regs *regs)
+static nokprobe_inline int watchpoint_report(struct perf_event *wp, unsigned long addr,
+					     struct pt_regs *regs)
 {
 	int step = is_default_overflow_handler(wp);
 	struct arch_hw_breakpoint *info = counter_arch_bp(wp);

-- 
2.53.0

[PATCH v4 3/6] arm64: hw_breakpoint: Add lockdep_assert_irqs_disabled() on install/uninstall

[Rob Herring (Arm)]

The breakpoint install/uninstall/restore code depends on interrupts
being disabled. Make this requirement explicit with a
lockdep_assert_irqs_disabled() assertion.

Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
 arch/arm64/kernel/hw_breakpoint.c | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index bb39bc759810..a9266dc710b4 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -231,6 +231,8 @@ static int hw_breakpoint_control(struct perf_event *bp,
 	enum dbg_active_el dbg_el = debug_exception_level(info->ctrl.privilege);
 	u32 ctrl;
 
+	lockdep_assert_irqs_disabled();
+
 	if (info->ctrl.type == ARM_BREAKPOINT_EXECUTE) {
 		/* Breakpoint */
 		ctrl_reg = AARCH64_DBG_REG_BCR;

-- 
2.53.0

[PATCH v4 4/6] arm64/cpufeature: Add field details for ID_AA64DFR1_EL1 register

[Rob Herring (Arm)]

From: Anshuman Khandual <anshuman.khandual@arm.com>

This adds required field details for ID_AA64DFR1_EL1, and also drops dummy
ftr_raz[] array which is now redundant. These register fields will be used
to enable increased breakpoint and watchpoint registers via FEAT_Debugv8p9
later. The register fields have been marked as FTR_STRICT, unless there is
a known variation in practice.

Signed-off-by: Anshuman Khandual <anshuman.khandual@arm.com>
Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
 arch/arm64/kernel/cpufeature.c | 21 ++++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

diff --git a/arch/arm64/kernel/cpufeature.c b/arch/arm64/kernel/cpufeature.c
index c31f8e17732a..24c8e9147e35 100644
--- a/arch/arm64/kernel/cpufeature.c
+++ b/arch/arm64/kernel/cpufeature.c
@@ -570,6 +570,21 @@ static const struct arm64_ftr_bits ftr_id_aa64dfr0[] = {
 	ARM64_FTR_END,
 };
 
+static const struct arm64_ftr_bits ftr_id_aa64dfr1[] = {
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_ABL_CMPs_SHIFT, 8, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_DPFZS_SHIFT, 4, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_STRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_EBEP_SHIFT, 4, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_STRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_ITE_SHIFT, 4, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_ABLE_SHIFT, 4, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_PMICNTR_SHIFT, 4, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_STRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_SPMU_SHIFT, 4, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_CTX_CMPs_SHIFT, 8, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_WRPs_SHIFT, 8, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_NONSTRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_BRPs_SHIFT, 8, 0),
+	ARM64_FTR_BITS(FTR_HIDDEN, FTR_STRICT, FTR_LOWER_SAFE, ID_AA64DFR1_EL1_SYSPMUID_SHIFT, 8, 0),
+	ARM64_FTR_END,
+};
+
 static const struct arm64_ftr_bits ftr_mvfr0[] = {
 	ARM64_FTR_BITS(FTR_HIDDEN, FTR_STRICT, FTR_LOWER_SAFE, MVFR0_EL1_FPRound_SHIFT, 4, 0),
 	ARM64_FTR_BITS(FTR_HIDDEN, FTR_STRICT, FTR_LOWER_SAFE, MVFR0_EL1_FPShVec_SHIFT, 4, 0),
@@ -756,10 +771,6 @@ static const struct arm64_ftr_bits ftr_single32[] = {
 	ARM64_FTR_END,
 };
 
-static const struct arm64_ftr_bits ftr_raz[] = {
-	ARM64_FTR_END,
-};
-
 #define __ARM64_FTR_REG_OVERRIDE(id_str, id, table, ovr) {	\
 		.sys_id = id,					\
 		.reg = 	&(struct arm64_ftr_reg){		\
@@ -832,7 +843,7 @@ static const struct __ftr_reg_entry {
 
 	/* Op1 = 0, CRn = 0, CRm = 5 */
 	ARM64_FTR_REG(SYS_ID_AA64DFR0_EL1, ftr_id_aa64dfr0),
-	ARM64_FTR_REG(SYS_ID_AA64DFR1_EL1, ftr_raz),
+	ARM64_FTR_REG(SYS_ID_AA64DFR1_EL1, ftr_id_aa64dfr1),
 
 	/* Op1 = 0, CRn = 0, CRm = 6 */
 	ARM64_FTR_REG(SYS_ID_AA64ISAR0_EL1, ftr_id_aa64isar0),

-- 
2.53.0

[PATCH v4 5/6] arm64/boot: Enable EL2 requirements for FEAT_Debugv8p9

[Rob Herring (Arm)]

From: Anshuman Khandual <anshuman.khandual@arm.com>

Fine grained trap control for MDSELR_EL1 register needs to be configured in
HDFGRTR2_EL2, and HDFGWTR2_EL2 registers when kernel enters at EL1, but EL2
is also present.

MDCR_EL2.EBWE needs to be enabled for additional (beyond 16) breakpoint and
watchpoint exceptions when kernel enters at EL1, but EL2 is also present.

While here, also update booting.rst with MDCR_EL3 and SCR_EL3 requirements.

Cc: Marc Zyngier <maz@kernel.org>
Cc: Oliver Upton <oliver.upton@linux.dev>
Cc: kvmarm@lists.linux.dev
Signed-off-by: Anshuman Khandual <anshuman.khandual@arm.com>
Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
v4:
 - Add that the requirements only apply when there are >16
   breakpoints/watchpoints
 - Adapt to changes in v7.0-rc1
---
 Documentation/arch/arm64/booting.rst | 13 +++++++++++++
 arch/arm64/include/asm/el2_setup.h   | 14 ++++++++++++++
 2 files changed, 27 insertions(+)

diff --git a/Documentation/arch/arm64/booting.rst b/Documentation/arch/arm64/booting.rst
index 13ef311dace8..00ba91bbd278 100644
--- a/Documentation/arch/arm64/booting.rst
+++ b/Documentation/arch/arm64/booting.rst
@@ -369,6 +369,19 @@ Before jumping into the kernel, the following conditions must be met:
     - ZCR_EL2.LEN must be initialised to the same value for all CPUs the
       kernel will execute on.
 
+  For CPUs with FEAT_Debugv8p9 extension present and >16 breakpoints or
+  watchpoints:
+
+  - If the kernel is entered at EL1 and EL2 is present:
+
+    - HDFGRTR2_EL2.nMDSELR_EL1 (bit 5) must be initialized to 0b1
+    - HDFGWTR2_EL2.nMDSELR_EL1 (bit 5) must be initialized to 0b1
+    - MDCR_EL2.EBWE (bit 43) must be initialized to 0b1
+
+  - If EL3 is present:
+
+    - MDCR_EL3.EBWE (bit 43) must be initialized to 0b1
+
   For CPUs with the Scalable Matrix Extension (FEAT_SME):
 
   - If EL3 is present:
diff --git a/arch/arm64/include/asm/el2_setup.h b/arch/arm64/include/asm/el2_setup.h
index 85f4c1615472..b51a280c18c0 100644
--- a/arch/arm64/include/asm/el2_setup.h
+++ b/arch/arm64/include/asm/el2_setup.h
@@ -174,6 +174,13 @@
 						// to own it.
 
 .Lskip_trace_\@:
+	mrs	x1, id_aa64dfr0_el1
+	ubfx	x1, x1, #ID_AA64DFR0_EL1_DebugVer_SHIFT, #4
+	cmp	x1, #ID_AA64DFR0_EL1_DebugVer_V8P9
+	b.lt	.Lskip_dbg_v8p9_\@
+
+	orr	x2, x2, #MDCR_EL2_EBWE
+.Lskip_dbg_v8p9_\@:
 	msr	mdcr_el2, x2			// Configure debug traps
 .endm
 
@@ -438,6 +445,13 @@
 	orr	x0, x0, #HDFGRTR2_EL2_nPMSDSFR_EL1
 
 .Lskip_spefds_\@:
+	mrs	x1, id_aa64dfr0_el1
+	ubfx	x1, x1, #ID_AA64DFR0_EL1_DebugVer_SHIFT, #4
+	cmp	x1, #ID_AA64DFR0_EL1_DebugVer_V8P9
+	b.lt	.Lskip_dbg_v8p9_\@
+
+	mov_q   x0, HDFGWTR2_EL2_nMDSELR_EL1
+.Lskip_dbg_v8p9_\@:
 	msr_s   SYS_HDFGRTR2_EL2, x0
 	msr_s   SYS_HDFGWTR2_EL2, x0
 	msr_s   SYS_HFGRTR2_EL2, xzr

-- 
2.53.0

[PATCH v4 6/6] arm64: hw_breakpoint: Enable FEAT_Debugv8p9

[Rob Herring (Arm)]

From: Anshuman Khandual <anshuman.khandual@arm.com>

Currently, there can be maximum 16 breakpoints and 16 watchpoints available
on a given platform - as detected from ID_AA64DFR0_EL1.[BRPs|WRPs] register
fields. These breakpoints and watchpoints can be extended further up to
64 via a new arch feature FEAT_Debugv8p9.

Checking for FEAT_Debugv8p9 alone is not enough to enable the support.
It is also necessary to determine if there are more than 16 breakpoints
or watchpoints. The behavior with FEAT_Debugv8p9 and <=16 breakpoints
and watchpoints is IMPDEF.

The addition of the MDSELR_EL1 to set the bank index makes the register
accesses non-atomic. However, the combination of all the breakpoint code
being in the kprobe blacklist and breakpoint install/uninstall being
protected by perf locking (IRQs disabled and context lock) will prevent
debug exceptions during accesses and serialize the accesses.

Signed-off-by: Anshuman Khandual <anshuman.khandual@arm.com>
Signed-off-by: Rob Herring (Arm) <robh@kernel.org>
---
v4:
 - Update commit message.
 - Configure MDSCR_EL1_EMBWE on CPU reset/hotplug instead of every time
   breakpoints are enabled/disabled.
 - Drop unnecessary IRQ save and restore on register accesses.
 - Stash checking whether FEAT_Debugv8p9 is used rather than reading
   feature register on every register access.
 - Check that we're greater than or equal to Debug_v8p9 not just equal
   to.
 - Use is_debug_v8p9_enabled() in get_num_brps/get_num_wrps(). Handle
   the case when FEAT_Debugv8p9 is present, but the number of BP/WP
   are <16. It is IMPDEF if ID_AA64DFR1_EL1 is used in this case. It is
   also IMPDEF if MDSELR_EL1 is accessible. TF-A doesn't enable access
   to MDSELR_EL1 in this case.
 - Mark register access functions nokprobe.
---
 arch/arm64/include/asm/hw_breakpoint.h | 47 ++++++++++++++++++++++++++--------
 arch/arm64/kernel/debug-monitors.c     | 16 ++++++++----
 arch/arm64/kernel/hw_breakpoint.c      | 41 +++++++++++++++++++++++++++--
 3 files changed, 87 insertions(+), 17 deletions(-)

diff --git a/arch/arm64/include/asm/hw_breakpoint.h b/arch/arm64/include/asm/hw_breakpoint.h
index bd81cf17744a..c5624a906f3c 100644
--- a/arch/arm64/include/asm/hw_breakpoint.h
+++ b/arch/arm64/include/asm/hw_breakpoint.h
@@ -79,8 +79,9 @@ static inline void decode_ctrl_reg(u32 reg,
  * Limits.
  * Changing these will require modifications to the register accessors.
  */
-#define ARM_MAX_BRP		16
-#define ARM_MAX_WRP		16
+#define ARM_MAX_BRP		64
+#define ARM_MAX_WRP		64
+#define MAX_PER_BANK		16
 
 /* Virtual debug register bases. */
 #define AARCH64_DBG_REG_BVR	0
@@ -94,6 +95,14 @@ static inline void decode_ctrl_reg(u32 reg,
 #define AARCH64_DBG_REG_NAME_WVR	wvr
 #define AARCH64_DBG_REG_NAME_WCR	wcr
 
+static inline bool is_debug_v8p9_enabled(void)
+{
+	u64 dfr0 = read_sanitised_ftr_reg(SYS_ID_AA64DFR0_EL1);
+	int dver = cpuid_feature_extract_unsigned_field(dfr0, ID_AA64DFR0_EL1_DebugVer_SHIFT);
+
+	return dver >= ID_AA64DFR0_EL1_DebugVer_V8P9;
+}
+
 /* Accessor macros for the debug registers. */
 #define AARCH64_DBG_READ(N, REG, VAL) do {\
 	VAL = read_sysreg(dbg##REG##N##_el1);\
@@ -138,19 +147,37 @@ static inline void ptrace_hw_copy_thread(struct task_struct *task)
 /* Determine number of BRP registers available. */
 static inline int get_num_brps(void)
 {
-	u64 dfr0 = read_sanitised_ftr_reg(SYS_ID_AA64DFR0_EL1);
-	return 1 +
-		cpuid_feature_extract_unsigned_field(dfr0,
-						ID_AA64DFR0_EL1_BRPs_SHIFT);
+	u64 dfr0, dfr1;
+	int brps;
+
+	dfr0 = read_sanitised_ftr_reg(SYS_ID_AA64DFR0_EL1);
+	brps = cpuid_feature_extract_unsigned_field(dfr0, ID_AA64DFR0_EL1_BRPs_SHIFT);
+	if (is_debug_v8p9_enabled() && brps == 15) {
+		dfr1 = read_sanitised_ftr_reg(SYS_ID_AA64DFR1_EL1);
+		brps = cpuid_feature_extract_unsigned_field_width(dfr1,
+								  ID_AA64DFR1_EL1_BRPs_SHIFT, 8);
+		if (!brps)
+			return 16;
+	}
+	return 1 + brps;
 }
 
 /* Determine number of WRP registers available. */
 static inline int get_num_wrps(void)
 {
-	u64 dfr0 = read_sanitised_ftr_reg(SYS_ID_AA64DFR0_EL1);
-	return 1 +
-		cpuid_feature_extract_unsigned_field(dfr0,
-						ID_AA64DFR0_EL1_WRPs_SHIFT);
+	u64 dfr0, dfr1;
+	int wrps;
+
+	dfr0 = read_sanitised_ftr_reg(SYS_ID_AA64DFR0_EL1);
+	wrps = cpuid_feature_extract_unsigned_field(dfr0, ID_AA64DFR0_EL1_WRPs_SHIFT);
+	if (is_debug_v8p9_enabled() && wrps == 15) {
+		dfr1 = read_sanitised_ftr_reg(SYS_ID_AA64DFR1_EL1);
+		wrps = cpuid_feature_extract_unsigned_field_width(dfr1,
+								  ID_AA64DFR1_EL1_WRPs_SHIFT, 8);
+		if (!wrps)
+			return 16;
+	}
+	return 1 + wrps;
 }
 
 #ifdef CONFIG_CPU_PM
diff --git a/arch/arm64/kernel/debug-monitors.c b/arch/arm64/kernel/debug-monitors.c
index 29307642f4c9..8ff74432d0c3 100644
--- a/arch/arm64/kernel/debug-monitors.c
+++ b/arch/arm64/kernel/debug-monitors.c
@@ -22,6 +22,7 @@
 #include <asm/daifflags.h>
 #include <asm/debug-monitors.h>
 #include <asm/exception.h>
+#include <asm/hw_breakpoint.h>
 #include <asm/kgdb.h>
 #include <asm/kprobes.h>
 #include <asm/system_misc.h>
@@ -123,11 +124,16 @@ void disable_debug_monitors(enum dbg_active_el el)
 }
 NOKPROBE_SYMBOL(disable_debug_monitors);
 
-/*
- * OS lock clearing.
- */
-static int clear_os_lock(unsigned int cpu)
+static int debug_monitors_reset(unsigned int cpu)
 {
+	if (is_debug_v8p9_enabled()) {
+		u64 mdscr = mdscr_read();
+
+		mdscr |= MDSCR_EL1_EMBWE;
+		mdscr_write(mdscr);
+	}
+
+	/* Clear OS lock */
 	write_sysreg(0, osdlr_el1);
 	write_sysreg(0, oslar_el1);
 	isb();
@@ -138,7 +144,7 @@ static int __init debug_monitors_init(void)
 {
 	return cpuhp_setup_state(CPUHP_AP_ARM64_DEBUG_MONITORS_STARTING,
 				 "arm64/debug_monitors:starting",
-				 clear_os_lock, NULL);
+				 debug_monitors_reset, NULL);
 }
 postcore_initcall(debug_monitors_init);
 
diff --git a/arch/arm64/kernel/hw_breakpoint.c b/arch/arm64/kernel/hw_breakpoint.c
index a9266dc710b4..ea48c1562bee 100644
--- a/arch/arm64/kernel/hw_breakpoint.c
+++ b/arch/arm64/kernel/hw_breakpoint.c
@@ -40,6 +40,7 @@ static DEFINE_PER_CPU(int, stepping_kernel_bp);
 /* Number of BRP/WRP registers on this CPU. */
 static int core_num_brps;
 static int core_num_wrps;
+static bool has_debug_v8p9;
 
 int hw_breakpoint_slots(int type)
 {
@@ -104,7 +105,7 @@ int hw_breakpoint_slots(int type)
 	WRITE_WB_REG_CASE(OFF, 14, REG, VAL);	\
 	WRITE_WB_REG_CASE(OFF, 15, REG, VAL)
 
-static u64 read_wb_reg(int reg, int n)
+static nokprobe_inline u64 __read_wb_reg(int reg, int n)
 {
 	u64 val = 0;
 
@@ -119,9 +120,27 @@ static u64 read_wb_reg(int reg, int n)
 
 	return val;
 }
+
+static u64 read_wb_reg(int reg, int n)
+{
+	u64 val;
+
+	/*
+	 * Bank selection in MDSELR_EL1, followed by an indexed read from
+	 * breakpoint (or watchpoint) registers cannot be interrupted, as
+	 * that might cause misread from the wrong targets instead. Hence
+	 * this requires mutual exclusion.
+	 */
+	if (has_debug_v8p9) {
+		write_sysreg_s(SYS_FIELD_PREP(MDSELR_EL1, BANK, n / MAX_PER_BANK), SYS_MDSELR_EL1);
+		isb();
+	}
+	val = __read_wb_reg(reg, n % MAX_PER_BANK);
+	return val;
+}
 NOKPROBE_SYMBOL(read_wb_reg);
 
-static void write_wb_reg(int reg, int n, u64 val)
+static nokprobe_inline void __write_wb_reg(int reg, int n, u64 val)
 {
 	switch (reg + n) {
 	GEN_WRITE_WB_REG_CASES(AARCH64_DBG_REG_BVR, AARCH64_DBG_REG_NAME_BVR, val);
@@ -133,6 +152,21 @@ static void write_wb_reg(int reg, int n, u64 val)
 	}
 	isb();
 }
+
+static void write_wb_reg(int reg, int n, u64 val)
+{
+	/*
+	 * Bank selection in MDSELR_EL1, followed by an indexed read from
+	 * breakpoint (or watchpoint) registers cannot be interrupted, as
+	 * that might cause misread from the wrong targets instead. Hence
+	 * this requires mutual exclusion.
+	 */
+	if (has_debug_v8p9) {
+		write_sysreg_s(SYS_FIELD_PREP(MDSELR_EL1, BANK, n / MAX_PER_BANK), SYS_MDSELR_EL1);
+		isb();
+	}
+	__write_wb_reg(reg, n % MAX_PER_BANK, val);
+}
 NOKPROBE_SYMBOL(write_wb_reg);
 
 /*
@@ -990,6 +1024,7 @@ static int __init arch_hw_breakpoint_init(void)
 
 	core_num_brps = get_num_brps();
 	core_num_wrps = get_num_wrps();
+	has_debug_v8p9 = (core_num_brps > 16) || (core_num_wrps > 16);
 
 	pr_info("found %d breakpoint and %d watchpoint registers.\n",
 		core_num_brps, core_num_wrps);
@@ -1006,6 +1041,8 @@ static int __init arch_hw_breakpoint_init(void)
 
 	/* Register cpu_suspend hw breakpoint restore hook */
 	cpu_suspend_set_dbg_restorer(hw_breakpoint_reset);
+	BUILD_BUG_ON((ARM_MAX_BRP % MAX_PER_BANK) != 0);
+	BUILD_BUG_ON((ARM_MAX_WRP % MAX_PER_BANK) != 0);
 
 	return ret;
 }

-- 
2.53.0