From mboxrd@z Thu Jan  1 00:00:00 1970
From: Xiang W <wxjstz@126.com>
Date: Sun, 11 Dec 2022 00:54:21 +0800
Subject: [PATCH RFC v2 2/2] lib: sbi: add support for debug triggers
In-Reply-To: <20221203213929.206429-3-geomatsi@gmail.com>
References: <20221203213929.206429-1-geomatsi@gmail.com>
	 <20221203213929.206429-3-geomatsi@gmail.com>
Message-ID: <92833f1aeda3262f4ccb4b7fc796ee4de36c7ef0.camel@126.com>
List-Id: <opensbi.lists.infradead.org>
To: opensbi@lists.infradead.org
MIME-Version: 1.0
Content-Type: text/plain; charset="us-ascii"
Content-Transfer-Encoding: 7bit

? 2022-12-04???? 00:39 +0300?Sergey Matyukevich???
> From: Sergey Matyukevich <sergey.matyukevich@syntacore.com>
> 
> RISC-V Debug specification includes Sdtrig ISA extension.
> This extension describes Trigger Module. Triggers can cause
> a breakpoint exception, entry into Debug Mode, or a trace
> action without having to execute a special instruction. For
> native debugging triggers can be used to implement hardware
> breakpoints and watchpoints.
> 
> Software support for triggers consists of the following
> major components:
> - U-mode: gdb
> - S-mode: hardware breakpoints framework in Linux kernel
> - M-mode: SBI firmware code to handle triggers
> 
> SBI Debug Trigger extension proposal has been posted by
> Anup Patel to lists.riscv.org tech-debug mailing list:
> https://lists.riscv.org/g/tech-debug/topic/92375492
> 
> This patch provides initial implementation of SBI Debug
> Trigger Extension in OpenSBI library based on the
> suggested extension proposal.
> 
> Initial implementation has the following limitations:
> - supported triggers: mcontrol, mcontrol6
> - no support for chained triggers
> - only build test for RV32
> 
> ---
> ?include/sbi/riscv_dbtr.h????????? | 179 +++++++++++
> ?include/sbi/riscv_encoding.h????? |?? 1 +
> ?include/sbi/sbi_dbtr.h??????????? |? 80 +++++
> ?include/sbi/sbi_ecall_interface.h |? 10 +
> ?lib/sbi/Kconfig?????????????????? |?? 4 +
> ?lib/sbi/objects.mk??????????????? |?? 4 +
> ?lib/sbi/sbi_dbtr.c??????????????? | 481 ++++++++++++++++++++++++++++++
> ?lib/sbi/sbi_ecall_dbtr.c????????? |? 68 +++++
> ?lib/sbi/sbi_init.c??????????????? |?? 9 +
> ?9 files changed, 836 insertions(+)
> ?create mode 100644 include/sbi/riscv_dbtr.h
> ?create mode 100644 include/sbi/sbi_dbtr.h
> ?create mode 100644 lib/sbi/sbi_dbtr.c
> ?create mode 100644 lib/sbi/sbi_ecall_dbtr.c
> 
> diff --git a/include/sbi/riscv_dbtr.h b/include/sbi/riscv_dbtr.h
> new file mode 100644
> index 0000000..9944547
> --- /dev/null
> +++ b/include/sbi/riscv_dbtr.h
> @@ -0,0 +1,179 @@
> +/*
> + * SPDX-License-Identifier: BSD-2-Clause
> + *
> + * Copyright (c) 2022 Syntacore
> + *
> + * Authors:
> + *?? Sergey Matyukevich <sergey.matyukevich@syntacore.com>
> + *
> + */
> +
> +#ifndef __RISCV_DBTR_H__
> +#define __RISCV_DBTR_H__
> +
> +#define RV_MAX_TRIGGERS????????32
> +
> +enum {
> +???????RISCV_DBTR_TRIG_NONE = 0,
> +???????RISCV_DBTR_TRIG_LEGACY,
> +???????RISCV_DBTR_TRIG_MCONTROL,
> +???????RISCV_DBTR_TRIG_ICOUNT,
> +???????RISCV_DBTR_TRIG_ITRIGGER,
> +???????RISCV_DBTR_TRIG_ETRIGGER,
> +???????RISCV_DBTR_TRIG_MCONTROL6,
> +};
> +
> +union riscv_dbtr_tdata1 {
> +???????unsigned long value;
> +???????struct {
> +#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
> +???????????????unsigned long type:4;
> +???????????????unsigned long dmode:1;
> +#if __riscv_xlen == 64
> +???????????????unsigned long data:59;
> +#elif __riscv_xlen == 32
> +???????????????unsigned long data:27;
> +#else
> +#error "Unexpected __riscv_xlen"
> +#endif
> +#elif __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
> +#if __riscv_xlen == 64
> +???????????????unsigned long data:59;
> +#elif __riscv_xlen == 32
> +???????????????unsigned long data:27;
> +#else
> +#error "Unexpected __riscv_xlen"
> +#endif
> +???????????????unsigned long dmode:1;
> +???????????????unsigned long type:4;
> +#else
> +#error "Unexpected endianness"
> +#endif
> +???????};
> +};
> +
> +union riscv_dbtr_tdata1_mcontrol {
> +???????unsigned long value;
> +???????struct {
> +#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
> +???????????????unsigned long type:4;
> +???????????????unsigned long dmode:1;
> +???????????????unsigned long maskmax:6;
> +#if __riscv_xlen >= 64
> +???????????????unsigned long _res1:30;
> +???????????????unsigned long sizehi:2;
> +#endif
> +???????????????unsigned long hit:1;
> +???????????????unsigned long select:1;
> +???????????????unsigned long timing:1;
> +???????????????unsigned long sizelo:2;
> +???????????????unsigned long action:4;
> +???????????????unsigned long chain:1;
> +???????????????unsigned long match:4;
> +???????????????unsigned long m:1;
> +???????????????unsigned long _res2:1;
> +???????????????unsigned long s:1;
> +???????????????unsigned long u:1;
> +???????????????unsigned long execute:1;
> +???????????????unsigned long store:1;
> +???????????????unsigned long load:1;
> +#elif __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
> +???????????????unsigned long load:1;
> +???????????????unsigned long store:1;
> +???????????????unsigned long execute:1;
> +???????????????unsigned long u:1;
> +???????????????unsigned long s:1;
> +???????????????unsigned long _res2:1;
> +???????????????unsigned long m:1;
> +???????????????unsigned long match:4;
> +???????????????unsigned long chain:1;
> +???????????????unsigned long action:4;
> +???????????????unsigned long sizelo:2;
> +???????????????unsigned long timing:1;
> +???????????????unsigned long select:1;
> +???????????????unsigned long hit:1;
> +#if __riscv_xlen >= 64
> +???????????????unsigned long sizehi:2;
> +???????????????unsigned long _res1:30;
> +#endif
> +???????????????unsigned long maskmax:6;
> +???????????????unsigned long dmode:1;
> +???????????????unsigned long type:4;
> +#else
> +#error "Unexpected endianness"
> +#endif
> +???????};
> +};
> +
> +#define MCONTROL_U_OFFSET??????3
> +#define MCONTROL_S_OFFSET??????4
> +#define MCONTROL_M_OFFSET??????6
> +
> +union riscv_dbtr_tdata1_mcontrol6 {
> +???????unsigned long value;
> +???????struct {
> +#if __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
> +???????????????unsigned long type:4;
> +???????????????unsigned long dmode:1;
> +#if __riscv_xlen == 64
> +???????????????unsigned long _res1:34;
> +#elif __riscv_xlen == 32
> +???????????????unsigned long _res1:2;
> +#else
> +#error "Unexpected __riscv_xlen"
> +#endif
> +???????????????unsigned long vs:1;
> +???????????????unsigned long vu:1;
> +???????????????unsigned long hit:1;
> +???????????????unsigned long select:1;
> +???????????????unsigned long timing:1;
> +???????????????unsigned long size:4;
> +???????????????unsigned long action:4;
> +???????????????unsigned long chain:1;
> +???????????????unsigned long match:4;
> +???????????????unsigned long m:1;
> +???????????????unsigned long _res2:1;
> +???????????????unsigned long s:1;
> +???????????????unsigned long u:1;
> +???????????????unsigned long execute:1;
> +???????????????unsigned long store:1;
> +???????????????unsigned long load:1;
> +#elif __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
> +???????????????unsigned long load:1;
> +???????????????unsigned long store:1;
> +???????????????unsigned long execute:1;
> +???????????????unsigned long u:1;
> +???????????????unsigned long s:1;
> +???????????????unsigned long _res2:1;
> +???????????????unsigned long m:1;
> +???????????????unsigned long match:4;
> +???????????????unsigned long chain:1;
> +???????????????unsigned long action:4;
> +???????????????unsigned long size:4;
> +???????????????unsigned long timing:1;
> +???????????????unsigned long select:1;
> +???????????????unsigned long hit:1;
> +???????????????unsigned long vu:1;
> +???????????????unsigned long vs:1;
> +#if __riscv_xlen == 64
> +???????????????unsigned long _res1:34;
> +#elif __riscv_xlen == 32
> +???????????????unsigned long _res1:2;
> +#else
> +#error "Unexpected __riscv_xlen"
> +#endif
> +???????????????unsigned long dmode:1;
> +???????????????unsigned long type:4;
> +#else
> +#error "Unexpected endianness"
> +#endif
> +???????};
> +};
> +
> +#define MCONTROL6_U_OFFSET?????3
> +#define MCONTROL6_S_OFFSET?????4
> +#define MCONTROL6_M_OFFSET?????6
> +#define MCONTROL6_VU_OFFSET????23
> +#define MCONTROL6_VS_OFFSET????24
> +
> +#endif /* __RISCV_DBTR_H__ */
> diff --git a/include/sbi/riscv_encoding.h b/include/sbi/riscv_encoding.h
> index b0f08c8..2041ab7 100644
> --- a/include/sbi/riscv_encoding.h
> +++ b/include/sbi/riscv_encoding.h
> @@ -671,6 +671,7 @@
> ?#define CSR_TDATA1?????????????????????0x7a1
> ?#define CSR_TDATA2?????????????????????0x7a2
> ?#define CSR_TDATA3?????????????????????0x7a3
> +#define CSR_TINFO??????????????????????0x7a4
> ?
> ?/* Debug Mode Registers */
> ?#define CSR_DCSR???????????????????????0x7b0
> diff --git a/include/sbi/sbi_dbtr.h b/include/sbi/sbi_dbtr.h
> new file mode 100644
> index 0000000..5ef6166
> --- /dev/null
> +++ b/include/sbi/sbi_dbtr.h
> @@ -0,0 +1,80 @@
> +/*
> + * SPDX-License-Identifier: BSD-2-Clause
> + *
> + * Copyright (c) 2022 Syntacore
> + *
> + * Authors:
> + *?? Sergey Matyukevich <sergey.matyukevich@syntacore.com>
> + *
> + */
> +
> +#ifndef __SBI_DBTR_H__
> +#define __SBI_DBTR_H__
> +
> +#include <sbi/riscv_endian.h>
> +#include <sbi/riscv_dbtr.h>
> +
> +#include <sbi/sbi_hartmask.h>
> +#include <sbi/sbi_scratch.h>
> +#include <sbi/sbi_domain.h>
> +#include <sbi/sbi_types.h>
> +
> +/** Representation of trigger state */
> +union sbi_dbtr_trig_state {
> +???????unsigned long value;
> +???????struct {
> +???????????????unsigned long mapped:1;
> +???????????????unsigned long u:1;
> +???????????????unsigned long s:1;
> +???????????????unsigned long vu:1;
> +???????????????unsigned long vs:1;
> +#if __riscv_xlen == 64
> +???????????????unsigned long reserved:59;
> +#elif __riscv_xlen == 32
> +???????????????unsigned long reserved:27;
> +#else
> +#error "Unexpected __riscv_xlen"
> +#endif
> +???????};
> +};
> +
> +struct sbi_dbtr_trig_info {
> +???????unsigned long type_mask;
> +???????union sbi_dbtr_trig_state state;
> +???????unsigned long tdata1;
> +???????unsigned long tdata2;
> +???????unsigned long tdata3;
tdata1~tdata3 can be removed and written directly to scr during install
> +};
> +
> +/** SBI shared mem messages layout */
> +struct sbi_dbtr_data_msg {
> +???????unsigned long tstate;
> +???????unsigned long tdata1;
> +???????unsigned long tdata2;
> +???????unsigned long tdata3;
> +};
> +
> +struct sbi_dbtr_id_msg {
> +???????unsigned long idx;
> +};
> +
> +/** Initialize PMU */
> +int sbi_dbtr_init(struct sbi_scratch *scratch);
> +
> +/** SBI DBTR extension functions */
> +int sbi_dbtr_probe(unsigned long *out);
> +int sbi_dbtr_num_trig(unsigned long trig_tdata1, unsigned long *out);
> +int sbi_dbtr_read_trig(const struct sbi_domain *dom, unsigned long smode,
> +???????????????unsigned long trig_idx_base, unsigned long trig_count,
> +???????????????unsigned long out_addr_div_by_16);
> +int sbi_dbtr_install_trig(const struct sbi_domain *dom, unsigned long smode,
> +???????????????unsigned long trig_count, unsigned long in_addr_div_by_16,
> +???????????????unsigned long out_addr_div_by_16, unsigned long *out);
> +int sbi_dbtr_uninstall_trig(unsigned long trig_idx_base, unsigned long trig_idx_mask);
> +int sbi_dbtr_enable_trig(unsigned long trig_idx_base, unsigned long trig_idx_mask);
> +int sbi_dbtr_update_trig(const struct sbi_domain *dom, unsigned long smode,
> +???????????????unsigned long trig_count, unsigned long in_addr_div_by_16,
> +???????????????unsigned long out_addr_div_by_16);
> +int sbi_dbtr_disable_trig(unsigned long trig_idx_base, unsigned long trig_idx_mask);
> +
> +#endif
> diff --git a/include/sbi/sbi_ecall_interface.h b/include/sbi/sbi_ecall_interface.h
> index a3f2bf4..3c1b1ea 100644
> --- a/include/sbi/sbi_ecall_interface.h
> +++ b/include/sbi/sbi_ecall_interface.h
> @@ -29,6 +29,7 @@
> ?#define SBI_EXT_HSM????????????????????????????0x48534D
> ?#define SBI_EXT_SRST???????????????????????????0x53525354
> ?#define SBI_EXT_PMU????????????????????????????0x504D55
> +#define SBI_EXT_DBTR???????????????????????????0x44425452
> ?
> ?/* SBI function IDs for BASE extension*/
> ?#define SBI_EXT_BASE_GET_SPEC_VERSION??????????0x0
> @@ -100,6 +101,15 @@
> ?#define SBI_EXT_PMU_COUNTER_STOP???????0x4
> ?#define SBI_EXT_PMU_COUNTER_FW_READ????0x5
> ?
> +/* SBI function IDs for DBTR extension */
> +#define SBI_EXT_DBTR_NUM_TRIGGERS??????0x0
> +#define SBI_EXT_DBTR_TRIGGER_READ??????0x1
> +#define SBI_EXT_DBTR_TRIGGER_INSTALL???0x2
> +#define SBI_EXT_DBTR_TRIGGER_UNINSTALL?0x3
> +#define SBI_EXT_DBTR_TRIGGER_ENABLE????0x4
> +#define SBI_EXT_DBTR_TRIGGER_UPDATE????0x5
> +#define SBI_EXT_DBTR_TRIGGER_DISABLE???0x6
> +
> ?/** General pmu event codes specified in SBI PMU extension */
> ?enum sbi_pmu_hw_generic_events_t {
> ????????SBI_PMU_HW_NO_EVENT?????????????????????= 0,
> diff --git a/lib/sbi/Kconfig b/lib/sbi/Kconfig
> index df74bba..e3a7955 100644
> --- a/lib/sbi/Kconfig
> +++ b/lib/sbi/Kconfig
> @@ -34,4 +34,8 @@ config SBI_ECALL_VENDOR
> ????????bool "Platform-defined vendor extensions"
> ????????default y
> ?
> +config SBI_ECALL_DBTR
> +???????bool "Debug Trigger Extension"
> +???????default y
> +
> ?endmenu
> diff --git a/lib/sbi/objects.mk b/lib/sbi/objects.mk
> index c774ebb..36276e8 100644
> --- a/lib/sbi/objects.mk
> +++ b/lib/sbi/objects.mk
> @@ -43,6 +43,9 @@ libsbi-objs-$(CONFIG_SBI_ECALL_LEGACY) += sbi_ecall_legacy.o
> ?carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_VENDOR) += ecall_vendor
> ?libsbi-objs-$(CONFIG_SBI_ECALL_VENDOR) += sbi_ecall_vendor.o
> ?
> +carray-sbi_ecall_exts-$(CONFIG_SBI_ECALL_DBTR) += ecall_dbtr
> +libsbi-objs-$(CONFIG_SBI_ECALL_DBTR) += sbi_ecall_dbtr.o
> +
> ?libsbi-objs-y += sbi_bitmap.o
> ?libsbi-objs-y += sbi_bitops.o
> ?libsbi-objs-y += sbi_console.o
> @@ -60,6 +63,7 @@ libsbi-objs-y += sbi_irqchip.o
> ?libsbi-objs-y += sbi_misaligned_ldst.o
> ?libsbi-objs-y += sbi_platform.o
> ?libsbi-objs-y += sbi_pmu.o
> +libsbi-objs-y += sbi_dbtr.o
> ?libsbi-objs-y += sbi_scratch.o
> ?libsbi-objs-y += sbi_string.o
> ?libsbi-objs-y += sbi_system.o
> diff --git a/lib/sbi/sbi_dbtr.c b/lib/sbi/sbi_dbtr.c
> new file mode 100644
> index 0000000..bbad0cf
> --- /dev/null
> +++ b/lib/sbi/sbi_dbtr.c
> @@ -0,0 +1,481 @@
> +/*
> + * SPDX-License-Identifier: BSD-2-Clause
> + *
> + * Copyright (c) 2022 Syntacore
> + *
> + * Authors:
> + *?? Sergey Matyukevich <sergey.matyukevich@syntacore.com>
> + *
> + */
> +
> +#include <sbi/sbi_ecall_interface.h>
> +#include <sbi/sbi_csr_detect.h>
> +#include <sbi/sbi_platform.h>
> +#include <sbi/sbi_console.h>
> +#include <sbi/sbi_trap.h>
> +#include <sbi/sbi_dbtr.h>
> +
> +#include <sbi/riscv_encoding.h>
> +#include <sbi/riscv_asm.h>
> +
> +static struct sbi_dbtr_trig_info triggers[SBI_HARTMASK_MAX_BITS][RV_MAX_TRIGGERS] = {0};
> +static uint32_t total_trigs;
> +
> +static void sbi_triggers_table_init(u32 hartid, int idx, unsigned long type_mask)
> +{
> +???????triggers[hartid][idx].type_mask = type_mask;
> +???????triggers[hartid][idx].state.value = 0;
> +???????triggers[hartid][idx].tdata1 = 0;
> +???????triggers[hartid][idx].tdata2 = 0;
> +???????triggers[hartid][idx].tdata3 = 0;
> +}
> +
> +int sbi_dbtr_init(struct sbi_scratch *scratch)
> +{
> +???????struct sbi_trap_info trap = {0};
> +???????u32 hartid = current_hartid();
> +???????union riscv_dbtr_tdata1 tdata1;
> +???????unsigned long val;
> +???????int i;
> +
> +???????total_trigs = 0;
> +
> +???????for (i = 0; i < RV_MAX_TRIGGERS; i++) {
> +???????????????csr_write_allowed(CSR_TSELECT, (ulong)&trap, i);
> +???????????????if (trap.cause)
> +???????????????????????break;
> +
> +???????????????val = csr_read_allowed(CSR_TSELECT, (ulong)&trap);
> +???????????????if (trap.cause)
> +???????????????????????break;
> +
> +???????????????/* Read back tselect and check that it contains the written value */
> +???????????????if (val != i)
> +???????????????????????break;
> +
> +???????????????val = csr_read_allowed(CSR_TINFO, (ulong)&trap);
> +???????????????if (trap.cause) {
> +???????????????????????/**
> +??????????????????????? * If reading tinfo caused an exception, the debugger
> +??????????????????????? * must read tdata1 to discover the type.
> +??????????????????????? */
> +???????????????????????tdata1.value = csr_read_allowed(CSR_TDATA1, (ulong)&trap);
> +???????????????????????if (trap.cause)
> +???????????????????????????????break;
> +
> +???????????????????????if (tdata1.type == 0)
> +???????????????????????????????break;
> +
> +
> +???????????????????????sbi_triggers_table_init(hartid, i, BIT(tdata1.type));
> +???????????????????????total_trigs++;
> +???????????????} else {
> +???????????????????????if (val == 1)
> +???????????????????????????????break;
tinfo = 0 have to break, this is a invalid value.
> +
> +???????????????????????sbi_triggers_table_init(hartid, i, val);
> +???????????????????????total_trigs++;
> +???????????????}
> +???????}
> +
> +???????return 0;
> +}
> +
> +int sbi_dbtr_probe(unsigned long *out)
> +{
> +???????*out? = total_trigs;
> +
> +???????return 0;
> +}
> +
> +
> +static void dbtr_trigger_init(unsigned int hartid, unsigned int idx,
> +???????????????????????????? struct sbi_dbtr_data_msg *recv)
> +{
> +???????union riscv_dbtr_tdata1 tdata1;
> +
> +???????triggers[hartid][idx].tdata1 = lle_to_cpu(recv->tdata1);
> +???????triggers[hartid][idx].tdata2 = lle_to_cpu(recv->tdata2);
> +???????triggers[hartid][idx].tdata3 = lle_to_cpu(recv->tdata3);
> +???????triggers[hartid][idx].state.mapped = 1;
> +
> +???????tdata1.value = lle_to_cpu(recv->tdata1);
> +
> +???????switch (tdata1.type) {
> +???????case RISCV_DBTR_TRIG_MCONTROL:
> +???????????????triggers[hartid][idx].state.u = __test_bit(MCONTROL_U_OFFSET, &tdata1.value);
> +???????????????triggers[hartid][idx].state.s = __test_bit(MCONTROL_S_OFFSET, &tdata1.value);
> +???????????????triggers[hartid][idx].state.vu = 0;
> +???????????????triggers[hartid][idx].state.vs = 0;
> +???????????????break;
> +???????case RISCV_DBTR_TRIG_MCONTROL6:
> +???????????????triggers[hartid][idx].state.u = __test_bit(MCONTROL6_U_OFFSET, &tdata1.value);
> +???????????????triggers[hartid][idx].state.s = __test_bit(MCONTROL6_S_OFFSET, &tdata1.value);
> +???????????????triggers[hartid][idx].state.vu = __test_bit(MCONTROL6_VU_OFFSET, &tdata1.value);
> +???????????????triggers[hartid][idx].state.vs = __test_bit(MCONTROL6_VS_OFFSET, &tdata1.value);
> +???????????????break;
> +???????default:
> +???????????????break;
> +???????}
> +}
> +
> +static inline void update_bit(unsigned long new, int nr, volatile unsigned long *addr)
> +{
> +???????if (new)
> +???????????????__set_bit(nr, addr);
> +???????else
> +???????????????__clear_bit(nr, addr);
> +}
> +
> +static void dbtr_trigger_enable(unsigned int hartid, unsigned int idx)
> +{
> +???????union sbi_dbtr_trig_state state;
> +???????union riscv_dbtr_tdata1 tdata1;
> +
> +???????if (!triggers[hartid][idx].state.mapped)
> +???????????????return;
> +
> +???????state.value = triggers[hartid][idx].state.value;
> +???????tdata1.value = triggers[hartid][idx].tdata1;
> +
> +???????switch (tdata1.type) {
> +???????case RISCV_DBTR_TRIG_MCONTROL:
> +???????????????update_bit(state.u, MCONTROL_U_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????update_bit(state.s, MCONTROL_S_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????break;
> +???????case RISCV_DBTR_TRIG_MCONTROL6:
> +???????????????update_bit(state.vu, MCONTROL6_VU_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????update_bit(state.vs, MCONTROL6_VS_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????update_bit(state.u, MCONTROL6_U_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????update_bit(state.s, MCONTROL6_S_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????break;
> +???????default:
> +???????????????break;
> +???????}
> +
> +???????/*
> +??????? * RISC-V Debug Support v1.0.0 section 5.5:
> +??????? * Debugger cannot simply set a trigger by writing tdata1, then tdata2, etc. The current
> +??????? * value of tdata2 might not be legal with the new value of tdata1. To help with this
> +??????? * situation, it is guaranteed that writing 0 to tdata1 disables the trigger, and
> +??????? * leaves it in a state where tdata2 and tdata3 can be written with any value
> +??????? * that makes sense for any trigger type supported by this trigger.
> +??????? */
> +???????csr_write(CSR_TSELECT, idx);
> +???????csr_write(CSR_TDATA1, 0x0);
> +???????csr_write(CSR_TDATA2, triggers[hartid][idx].tdata2);
> +???????csr_write(CSR_TDATA1, triggers[hartid][idx].tdata1);
> +}
> +
> +static void dbtr_trigger_disable(unsigned int hartid, unsigned int idx)
> +{
> +???????union riscv_dbtr_tdata1 tdata1;
> +
> +???????if (!triggers[hartid][idx].state.mapped)
> +???????????????return;
> +
> +???????tdata1.value = triggers[hartid][idx].tdata1;
> +
> +???????switch (tdata1.type) {
> +???????case RISCV_DBTR_TRIG_MCONTROL:
> +???????????????__clear_bit(MCONTROL_U_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????__clear_bit(MCONTROL_S_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????break;
> +???????case RISCV_DBTR_TRIG_MCONTROL6:
> +???????????????__clear_bit(MCONTROL6_VU_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????__clear_bit(MCONTROL6_VS_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????__clear_bit(MCONTROL6_U_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????__clear_bit(MCONTROL6_S_OFFSET, &triggers[hartid][idx].tdata1);
> +???????????????break;
> +???????default:
> +???????????????break;
> +???????}
> +
> +???????csr_write(CSR_TSELECT, idx);
> +???????csr_write(CSR_TDATA1, triggers[hartid][idx].tdata1);
> +}
> +
> +static void dbtr_trigger_clear(unsigned int hartid, unsigned int idx)
> +{
> +???????if (!triggers[hartid][idx].state.mapped)
> +???????????????return;
> +
> +???????csr_write(CSR_TSELECT, idx);
> +???????csr_write(CSR_TDATA1, 0x0);
> +???????csr_write(CSR_TDATA2, 0x0);
> +}
> +
> +static int dbtr_trigger_supported(unsigned long type)
> +{
> +???????switch (type) {
> +???????case RISCV_DBTR_TRIG_MCONTROL:
> +???????case RISCV_DBTR_TRIG_MCONTROL6:
> +???????????????return 1;
> +???????default:
> +???????????????break;
> +???????}
> +
> +???????return 0;
> +}
> +
> +static int dbtr_trigger_valid(unsigned long type, unsigned long tdata)
> +{
> +???????union riscv_dbtr_tdata1_mcontrol6 control6;
> +???????union riscv_dbtr_tdata1_mcontrol control;
> +
> +???????switch (type) {
> +???????case RISCV_DBTR_TRIG_MCONTROL:
> +???????????????control.value = tdata;
> +???????????????if (!control.action && !control.dmode && !control.m)
> +???????????????????????return 1;
> +???????????????break;
> +???????case RISCV_DBTR_TRIG_MCONTROL6:
> +???????????????control6.value = tdata;
> +???????????????if (!control6.action && !control6.dmode && !control6.m)
> +???????????????????????return 1;
> +???????????????break;
> +???????default:
> +???????????????break;
> +???????}
> +
> +???????return 0;
> +}
> +
> +int sbi_dbtr_num_trig(unsigned long data, unsigned long *out)
> +{
> +???????unsigned long type = ((union riscv_dbtr_tdata1)data).type;
> +???????u32 hartid = current_hartid();
> +???????unsigned long total = 0;
> +???????int i;
> +
> +
> +???????if (data == 0) {
> +???????????????sbi_dprintf("%s: hart%d: total triggers: %u\n",
> +?????????????????????????? __func__, hartid, total_trigs);
> +???????????????*out = total_trigs;
> +???????????????return SBI_SUCCESS;
> +???????}
> +
> +???????for (i = 0; i < total_trigs; i++) {
> +???????????????if (__test_bit(type, &triggers[hartid][i].type_mask))
> +???????????????????????total++;
> +???????}
> +
> +
> +???????sbi_dprintf("%s: hart%d: total triggers of type %lu: %lu\n",
> +?????????????????? __func__, hartid, type, total);
> +
> +???????*out = total;
> +???????return SBI_SUCCESS;
> +}
> +
> +int sbi_dbtr_read_trig(const struct sbi_domain *dom, unsigned long smode,
> +???????????????unsigned long trig_idx_base, unsigned long trig_count,
> +???????????????unsigned long out_addr_div_by_16)
> +{
> +???????unsigned long out_addr = (out_addr_div_by_16 << 4);
> +???????struct sbi_dbtr_data_msg *xmit;
> +???????u32 hartid = current_hartid();
> +???????int i;
> +
> +???????if (smode != PRV_S)
> +???????????????return SBI_ERR_DENIED;
> +???????if (dom && !sbi_domain_is_assigned_hart(dom, hartid))
> +???????????????return SBI_ERR_DENIED;
> +???????if (dom && !sbi_domain_check_addr(dom, out_addr, smode, SBI_DOMAIN_READ | SBI_DOMAIN_WRITE))
> +???????????????return SBI_ERR_INVALID_ADDRESS;
> +
> +???????if (trig_idx_base >= total_trigs || trig_idx_base + trig_count >= total_trigs) {
> +???????????????sbi_dprintf("%s: hart%d: invalid trigger index\n", __func__, hartid);
> +???????????????return SBI_ERR_INVALID_PARAM;
> +???????}
> +
> +???????for (i = 0; i < trig_count; i++) {
> +???????????????xmit = (struct sbi_dbtr_data_msg *)(out_addr + i * sizeof(*xmit));
> +
> +???????????????sbi_dprintf("%s: hart%d: read trigger %d\n", __func__, hartid, i);
> +
> +???????????????xmit->tstate = cpu_to_lle(triggers[hartid][i + trig_idx_base].state.value);
> +???????????????xmit->tdata1 = cpu_to_lle(triggers[hartid][i + trig_idx_base].tdata1);
> +???????????????xmit->tdata2 = cpu_to_lle(triggers[hartid][i + trig_idx_base].tdata2);
> +???????????????xmit->tdata3 = cpu_to_lle(triggers[hartid][i + trig_idx_base].tdata3);
> +???????}
> +
> +???????return SBI_SUCCESS;
> +}
> +
> +int sbi_dbtr_install_trig(const struct sbi_domain *dom, unsigned long smode,
> +???????????????unsigned long trig_count, unsigned long in_addr_div_by_16,
> +???????????????unsigned long out_addr_div_by_16, unsigned long *out)
> +{
> +???????unsigned long out_addr = (out_addr_div_by_16 << 4);
> +???????unsigned long in_addr = (in_addr_div_by_16 << 4);
> +???????u32 hartid = current_hartid();
> +???????struct sbi_dbtr_data_msg *recv;
> +???????struct sbi_dbtr_id_msg *xmit;
> +???????union riscv_dbtr_tdata1 ctrl;
> +???????int i, k;
> +
> +???????if (smode != PRV_S)
> +???????????????return SBI_ERR_DENIED;
> +???????if (dom && !sbi_domain_is_assigned_hart(dom, hartid))
> +???????????????return SBI_ERR_DENIED;
> +???????if (dom && !sbi_domain_check_addr(dom, in_addr, smode, SBI_DOMAIN_READ | SBI_DOMAIN_WRITE))
> +???????????????return SBI_ERR_INVALID_ADDRESS;
> +???????if (dom && !sbi_domain_check_addr(dom, out_addr, smode, SBI_DOMAIN_READ | SBI_DOMAIN_WRITE))
> +???????????????return SBI_ERR_INVALID_ADDRESS;
> +
> +???????/* TODO: check chained triggers configurations */
> +
> +???????/* Check requested triggers configuration */
> +???????for (k = 0; k < trig_count; k++) {
> +???????????????recv = (struct sbi_dbtr_data_msg *)(in_addr + k * sizeof(*recv));
> +???????????????ctrl = (union riscv_dbtr_tdata1)recv->tdata1;
> +
> +???????????????if (!dbtr_trigger_supported(ctrl.type)) {
> +???????????????????????sbi_dprintf("%s: invalid type of trigger %d\n", __func__, k);
> +???????????????????????*out = k;
> +???????????????????????return SBI_ERR_FAILED;
> +???????????????}
> +
> +???????????????if (!dbtr_trigger_valid(ctrl.type, ctrl.value)) {
> +???????????????????????sbi_dprintf("%s: invalid configuration of trigger %d\n", __func__, k);
> +???????????????????????*out = k;
> +???????????????????????return SBI_ERR_FAILED;
> +???????????????}
> +???????}
> +
> +???????/* Check if we have enough spare triggers */
> +???????for (i = 0, k = 0; i < total_trigs; i++) {
> +???????????????if (!triggers[hartid][i].state.mapped)
> +???????????????????????k++;
> +???????}
> +
> +???????if (k < trig_count) {
> +???????????????sbi_dprintf("%s: hartid%d: not enough spare triggers\n", __func__, hartid);
> +???????????????*out = k;
> +???????????????return SBI_ERR_FAILED;
> +???????}
> +
> +???????/* Install triggers */
> +???????for (i = 0, k = 0; i < total_trigs; i++) {
> +???????????????if (triggers[hartid][i].state.mapped)
> +???????????????????????continue;
> +
> +???????????????recv = (struct sbi_dbtr_data_msg *)(in_addr + k * sizeof(*recv));
> +???????????????xmit = (struct sbi_dbtr_id_msg *)(out_addr + k * sizeof(*xmit));
> +
> +???????????????sbi_dprintf("%s: hart%d: idx[%d] tdata1[0x%lx] tdata2[0x%lx]\n",
> +?????????????????????????? __func__, hartid, i, recv->tdata1, recv->tdata2);
> +
> +???????????????dbtr_trigger_init(hartid, i,? recv);
> +???????????????dbtr_trigger_enable(hartid, i);
> +???????????????xmit->idx = cpu_to_lle(i);
> +
> +???????????????if (++k >= trig_count)
> +???????????????????????break;
> +???????}
> +
> +???????return SBI_SUCCESS;
> +}
> +
> +int sbi_dbtr_uninstall_trig(unsigned long trig_idx_base, unsigned long trig_idx_mask)
> +{
> +???????unsigned long trig_mask = trig_idx_mask << trig_idx_base;
> +???????unsigned long idx = trig_idx_base;
> +???????u32 hartid = current_hartid();
> +
> +???????sbi_dprintf("%s: hart%d: triggers mask: 0x%lx\n",
> +?????????????????? __func__, hartid, trig_mask);
> +
> +???????for_each_set_bit_from(idx, &trig_mask, total_trigs) {
> +???????????????if (!triggers[hartid][idx].state.mapped) {
> +???????????????????????sbi_dprintf("%s: trigger %lu not mapped\n", __func__, idx);
> +???????????????????????return SBI_ERR_INVALID_PARAM;
> +???????????????}
> +
> +???????????????sbi_dprintf("%s: clear trigger %lu\n", __func__, idx);
> +???????????????dbtr_trigger_clear(hartid, idx);
> +
> +???????????????triggers[hartid][idx].state.value = 0;
> +???????????????triggers[hartid][idx].tdata1 = 0;
> +???????????????triggers[hartid][idx].tdata2 = 0;
> +???????????????triggers[hartid][idx].tdata3 = 0;
> +???????}
> +
> +???????return SBI_SUCCESS;
> +}
> +
> +int sbi_dbtr_enable_trig(unsigned long trig_idx_base, unsigned long trig_idx_mask)
> +{
> +???????unsigned long trig_mask = trig_idx_mask << trig_idx_base;
> +???????unsigned long idx = trig_idx_base;
> +???????u32 hartid = current_hartid();
> +
> +???????sbi_dprintf("%s: hart%d: triggers mask: 0x%lx\n",
> +?????????????????? __func__, hartid, trig_mask);
> +
> +???????for_each_set_bit_from(idx, &trig_mask, total_trigs) {
> +???????????????sbi_dprintf("%s: enable trigger %lu\n", __func__, idx);
> +???????????????dbtr_trigger_enable(hartid, idx);
> +???????}
> +
> +???????return SBI_SUCCESS;
> +}
> +
> +int sbi_dbtr_update_trig(const struct sbi_domain *dom, unsigned long smode,
> +???????????????unsigned long trig_idx_base, unsigned long trig_idx_mask,
> +???????????????unsigned long in_addr_div_by_16)
> +{
> +???????unsigned long in_addr = (in_addr_div_by_16 << 4);
> +???????unsigned long trig_mask = trig_idx_mask << trig_idx_base;
> +???????unsigned long idx = trig_idx_base;
> +???????u32 hartid = current_hartid();
> +???????struct sbi_dbtr_data_msg *recv;
> +???????unsigned long uidx = 0;
> +
> +???????sbi_dprintf("%s: hart%d: triggers mask: 0x%lx\n",
> +?????????????????? __func__, hartid, trig_mask);
> +
> +???????if (smode != PRV_S)
> +???????????????return SBI_ERR_DENIED;
> +???????if (dom && !sbi_domain_is_assigned_hart(dom, hartid))
> +???????????????return SBI_ERR_DENIED;
> +???????if (dom && !sbi_domain_check_addr(dom, in_addr, smode, SBI_DOMAIN_READ | SBI_DOMAIN_WRITE))
> +???????????????return SBI_ERR_INVALID_ADDRESS;
> +
> +???????for_each_set_bit_from(idx, &trig_mask, total_trigs) {
> +???????????????if (!triggers[hartid][idx].state.mapped) {
> +???????????????????????sbi_dprintf("%s: trigger %lu not mapped\n", __func__, idx);
> +???????????????????????return SBI_ERR_INVALID_PARAM;
> +???????????????}
> +
> +???????????????recv = (struct sbi_dbtr_data_msg *)(in_addr + uidx * sizeof(*recv));
> +
> +???????????????sbi_dprintf("%s: update trigger %lu: newaddr 0x%lx\n",
> +?????????????????????????? __func__, idx, recv->tdata2);
> +
> +???????????????triggers[hartid][idx].tdata2 = lle_to_cpu(recv->tdata2);
> +???????????????dbtr_trigger_enable(hartid, idx);
> +???????????????uidx++;
> +???????}
> +
> +???????return SBI_SUCCESS;
> +}
> +
> +int sbi_dbtr_disable_trig(unsigned long trig_idx_base, unsigned long trig_idx_mask)
> +{
> +???????unsigned long trig_mask = trig_idx_mask << trig_idx_base;
> +???????unsigned long idx = trig_idx_base;
> +???????u32 hartid = current_hartid();
> +
> +???????sbi_dprintf("%s: hart%d: triggers mask: 0x%lx\n",
> +?????????????????? __func__, hartid, trig_mask);
> +
> +???????for_each_set_bit_from(idx, &trig_mask, total_trigs) {
> +???????????????sbi_dprintf("%s: disable trigger %lu\n", __func__, idx);
> +???????????????dbtr_trigger_disable(hartid, idx);
> +???????}
> +
> +???????return SBI_SUCCESS;
> +}
> diff --git a/lib/sbi/sbi_ecall_dbtr.c b/lib/sbi/sbi_ecall_dbtr.c
> new file mode 100644
> index 0000000..d013b3e
> --- /dev/null
> +++ b/lib/sbi/sbi_ecall_dbtr.c
> @@ -0,0 +1,68 @@
> +/*
> + * SPDX-License-Identifier: BSD-2-Clause
> + *
> + * Copyright (c) 2022 Syntacore
> + *
> + * Authors:
> + *?? Sergey Matyukevich <sergey.matyukevich@syntacore.com>
> + *
> + */
> +
> +#include <sbi/sbi_ecall_interface.h>
> +#include <sbi/sbi_ecall.h>
> +#include <sbi/sbi_domain.h>
> +#include <sbi/sbi_error.h>
> +#include <sbi/sbi_trap.h>
> +#include <sbi/sbi_version.h>
> +#include <sbi/sbi_dbtr.h>
> +
> +static int sbi_ecall_dbtr_handler(unsigned long extid, unsigned long funcid,
> +???????????????????????????????? const struct sbi_trap_regs *regs,
> +???????????????????????????????? unsigned long *out_val,
> +???????????????????????????????? struct sbi_trap_info *out_trap)
> +{
> +???????unsigned long smode = (csr_read(CSR_MSTATUS) & MSTATUS_MPP) >>
> +???????????????????????MSTATUS_MPP_SHIFT;
> +???????const struct sbi_domain *dom = sbi_domain_thishart_ptr();
> +???????int ret = 0;
> +
> +???????switch (funcid) {
> +???????case SBI_EXT_DBTR_NUM_TRIGGERS:
> +???????????????ret = sbi_dbtr_num_trig(regs->a0, out_val);
> +???????????????break;
> +???????case SBI_EXT_DBTR_TRIGGER_READ:
> +???????????????ret = sbi_dbtr_read_trig(dom, smode, regs->a0, regs->a1, regs->a2);
> +???????????????break;
> +???????case SBI_EXT_DBTR_TRIGGER_INSTALL:
> +???????????????ret = sbi_dbtr_install_trig(dom, smode, regs->a0, regs->a1, regs->a2, out_val);
> +???????????????break;
> +???????case SBI_EXT_DBTR_TRIGGER_UNINSTALL:
> +???????????????ret = sbi_dbtr_uninstall_trig(regs->a0, regs->a1);
> +???????????????break;
> +???????case SBI_EXT_DBTR_TRIGGER_ENABLE:
> +???????????????ret = sbi_dbtr_enable_trig(regs->a0, regs->a1);
> +???????????????break;
> +???????case SBI_EXT_DBTR_TRIGGER_UPDATE:
> +???????????????ret = sbi_dbtr_update_trig(dom, smode, regs->a0, regs->a1, regs->a2);
> +???????????????break;
> +???????case SBI_EXT_DBTR_TRIGGER_DISABLE:
> +???????????????ret = sbi_dbtr_disable_trig(regs->a0, regs->a1);
> +???????????????break;
> +???????default:
> +???????????????ret = SBI_ENOTSUPP;
> +???????};
> +
> +???????return ret;
> +}
> +
> +static int sbi_ecall_dbtr_probe(unsigned long extid, unsigned long *out_val)
> +{
> +???????return sbi_dbtr_probe(out_val);
> +}
> +
> +struct sbi_ecall_extension ecall_dbtr = {
> +???????.extid_start = SBI_EXT_DBTR,
> +???????.extid_end = SBI_EXT_DBTR,
> +???????.handle = sbi_ecall_dbtr_handler,
> +???????.probe = sbi_ecall_dbtr_probe,
> +};
> diff --git a/lib/sbi/sbi_init.c b/lib/sbi/sbi_init.c
> index a8500e5..956afae 100644
> --- a/lib/sbi/sbi_init.c
> +++ b/lib/sbi/sbi_init.c
> @@ -21,6 +21,7 @@
> ?#include <sbi/sbi_irqchip.h>
> ?#include <sbi/sbi_platform.h>
> ?#include <sbi/sbi_pmu.h>
> +#include <sbi/sbi_dbtr.h>
> ?#include <sbi/sbi_system.h>
> ?#include <sbi/sbi_string.h>
> ?#include <sbi/sbi_timer.h>
> @@ -275,6 +276,10 @@ static void __noreturn init_coldboot(struct sbi_scratch *scratch, u32 hartid)
> ????????if (rc)
> ????????????????sbi_hart_hang();
> ?
> +???????rc = sbi_dbtr_init(scratch);
> +???????if (rc)
> +???????????????sbi_hart_hang();
> +
> ????????sbi_boot_print_banner(scratch);
> ?
> ????????rc = sbi_irqchip_init(scratch, TRUE);
> @@ -380,6 +385,10 @@ static void init_warm_startup(struct sbi_scratch *scratch, u32 hartid)
> ????????if (rc)
> ????????????????sbi_hart_hang();
> ?
> +???????rc = sbi_dbtr_init(scratch);
> +???????if (rc)
> +???????????????sbi_hart_hang();
> +
> ????????rc = sbi_irqchip_init(scratch, FALSE);
> ????????if (rc)
> ????????????????sbi_hart_hang();
> -- 
> 2.38.1
> 
>