From cc7f99946dad984b33259a3db8426deda9989ddd Mon Sep 17 00:00:00 2001
From: Minda Chen <minda.chen@starfivetech.com>
Date: Mon, 20 Nov 2023 14:09:57 +0800
Subject: sbi: add amp ipi call function support

Add amp IPI call and set amp data address support.

Signed-off-by: Minda Chen <minda.chen@starfivetech.com>
---
 include/sbi/sbi_ecall_interface.h |  2 ++
 include/sbi/sbi_ipi.h             |  2 ++
 lib/sbi/sbi_ecall_ipi.c           |  5 +++++
 lib/sbi/sbi_ipi.c                 | 18 +++++++++++++++++-
 4 files changed, 26 insertions(+), 1 deletion(-)

diff --git a/include/sbi/sbi_ecall_interface.h b/include/sbi/sbi_ecall_interface.h
index 4597358..056915e 100644
--- a/include/sbi/sbi_ecall_interface.h
+++ b/include/sbi/sbi_ecall_interface.h
@@ -46,6 +46,8 @@
 
 /* SBI function IDs for IPI extension*/
 #define SBI_EXT_IPI_SEND_IPI			0x0
+#define SBI_EXT_IPI_SEND_EXT_DOMAIN		0x1
+#define SBI_EXT_IPI_SET_AMP_DATA_ADDR		0x2
 
 /* SBI function IDs for RFENCE extension*/
 #define SBI_EXT_RFENCE_REMOTE_FENCE_I		0x0
diff --git a/include/sbi/sbi_ipi.h b/include/sbi/sbi_ipi.h
index f384e74..24d9d18 100644
--- a/include/sbi/sbi_ipi.h
+++ b/include/sbi/sbi_ipi.h
@@ -68,6 +68,8 @@ int sbi_ipi_event_create(const struct sbi_ipi_event_ops *ops);
 void sbi_ipi_event_destroy(u32 event);
 
 int sbi_ipi_send_smode(ulong hmask, ulong hbase);
+int sbi_ipi_send_ext(u32 hartid, void *data, u32);
+void sbi_ipi_set_amp_data_addr(unsigned long addr);
 
 void sbi_ipi_clear_smode(void);
 
diff --git a/lib/sbi/sbi_ecall_ipi.c b/lib/sbi/sbi_ecall_ipi.c
index f4797e1..a50e84a 100644
--- a/lib/sbi/sbi_ecall_ipi.c
+++ b/lib/sbi/sbi_ecall_ipi.c
@@ -13,6 +13,7 @@
 #include <sbi/sbi_ecall_interface.h>
 #include <sbi/sbi_trap.h>
 #include <sbi/sbi_ipi.h>
+#include <sbi/sbi_console.h>
 
 static int sbi_ecall_ipi_handler(unsigned long extid, unsigned long funcid,
 				 const struct sbi_trap_regs *regs,
@@ -23,6 +24,10 @@ static int sbi_ecall_ipi_handler(unsigned long extid, unsigned long funcid,
 
 	if (funcid == SBI_EXT_IPI_SEND_IPI)
 		ret = sbi_ipi_send_smode(regs->a0, regs->a1);
+	else if (funcid == SBI_EXT_IPI_SEND_EXT_DOMAIN)
+		ret = sbi_ipi_send_ext(regs->a1, NULL, regs->a2);
+	else if (funcid == SBI_EXT_IPI_SET_AMP_DATA_ADDR)
+		sbi_ipi_set_amp_data_addr(regs->a0);
 	else
 		ret = SBI_ENOTSUPP;
 
diff --git a/lib/sbi/sbi_ipi.c b/lib/sbi/sbi_ipi.c
index b9f6205..101f73e 100644
--- a/lib/sbi/sbi_ipi.c
+++ b/lib/sbi/sbi_ipi.c
@@ -11,6 +11,7 @@
 #include <sbi/riscv_asm.h>
 #include <sbi/riscv_atomic.h>
 #include <sbi/riscv_barrier.h>
+#include <sbi/sbi_console.h>
 #include <sbi/sbi_bitops.h>
 #include <sbi/sbi_domain.h>
 #include <sbi/sbi_error.h>
@@ -150,12 +151,27 @@ static struct sbi_ipi_event_ops ipi_smode_ops = {
 };
 
 static u32 ipi_smode_event = SBI_IPI_EVENT_MAX;
-
+static unsigned long* amp_data_addr;
 int sbi_ipi_send_smode(ulong hmask, ulong hbase)
 {
 	return sbi_ipi_send_many(hmask, hbase, ipi_smode_event, NULL);
 }
 
+int sbi_ipi_send_ext(u32 hartid, void *data, u32 msg_bits)
+{
+	if (!amp_data_addr)
+		return SBI_EINVAL;
+
+	atomic_raw_set_bit((1 << msg_bits), (void *)(amp_data_addr + hartid));
+
+	return sbi_ipi_send(sbi_scratch_thishart_ptr(), hartid, ipi_smode_event, NULL);
+}
+
+void sbi_ipi_set_amp_data_addr(unsigned long addr)
+{
+	amp_data_addr = (void *)addr;
+}
+
 void sbi_ipi_clear_smode(void)
 {
 	csr_clear(CSR_MIP, MIP_SSIP);
-- 
cgit v1.2.3