4 files changed, 231 insertions, 91 deletions

diff --git a/sys/dev/firmware/arm/scmi.c b/sys/dev/firmware/arm/scmi.c
index fae4ec676a17..8428d17cecec 100644
--- a/sys/dev/firmware/arm/scmi.c
+++ b/sys/dev/firmware/arm/scmi.c
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: BSD-2-Clause
  *
  * Copyright (c) 2022 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2023 Arm Ltd
  *
  * This work was supported by Innovate UK project 105694, "Digital Security
  * by Design (DSbD) Technology Platform Prototype".
@@ -46,84 +47,62 @@
 
 #include "scmi.h"
 #include "scmi_protocols.h"
+#include "scmi_shmem.h"
 
-static device_t
-scmi_get_shmem(struct scmi_softc *sc, int index)
-{
-	phandle_t *shmems;
-	phandle_t node;
-	device_t dev;
-	size_t len;
-
-	node = ofw_bus_get_node(sc->dev);
-	if (node <= 0)
-		return (NULL);
-
-	len = OF_getencprop_alloc_multi(node, "shmem", sizeof(*shmems),
-	    (void **)&shmems);
-	if (len <= 0) {
-		device_printf(sc->dev, "%s: Can't get shmem node.\n", __func__);
-		return (NULL);
-	}
+#define	SCMI_HDR_TOKEN_S		18
+#define SCMI_HDR_TOKEN_BF		(0x3fff)
+#define	SCMI_HDR_TOKEN_M		(SCMI_HDR_TOKEN_BF << SCMI_HDR_TOKEN_S)
 
-	if (index >= len) {
-		OF_prop_free(shmems);
-		return (NULL);
-	}
+#define	SCMI_HDR_PROTOCOL_ID_S		10
+#define	SCMI_HDR_PROTOCOL_ID_BF		(0xff)
+#define	SCMI_HDR_PROTOCOL_ID_M		\
+    (SCMI_HDR_PROTOCOL_ID_BF << SCMI_HDR_PROTOCOL_ID_S)
 
-	dev = OF_device_from_xref(shmems[index]);
-	if (dev == NULL)
-		device_printf(sc->dev, "%s: Can't get shmem device.\n",
-		    __func__);
+#define	SCMI_HDR_MESSAGE_TYPE_S		8
+#define	SCMI_HDR_MESSAGE_TYPE_BF	(0x3)
+#define	SCMI_HDR_MESSAGE_TYPE_M		\
+    (SCMI_HDR_MESSAGE_TYPE_BF << SCMI_HDR_MESSAGE_TYPE_S)
 
-	OF_prop_free(shmems);
+#define	SCMI_HDR_MESSAGE_ID_S		0
+#define	SCMI_HDR_MESSAGE_ID_BF		(0xff)
+#define	SCMI_HDR_MESSAGE_ID_M		\
+    (SCMI_HDR_MESSAGE_ID_BF << SCMI_HDR_MESSAGE_ID_S)
 
-	return (dev);
-}
+#define SCMI_MSG_TYPE_CMD	0
+#define SCMI_MSG_TYPE_DRESP	2
+#define SCMI_MSG_TYPE_NOTIF	3
 
 static int
 scmi_request_locked(struct scmi_softc *sc, struct scmi_req *req)
 {
-	struct scmi_smt_header hdr;
+	uint32_t reply_header;
 	int ret;
 
-	bzero(&hdr, sizeof(struct scmi_smt_header));
-
 	SCMI_ASSERT_LOCKED(sc);
 
-	/* Read header */
-	scmi_shmem_read(sc->tx_shmem, 0, &hdr, SMT_HEADER_SIZE);
-
-	if ((hdr.channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE) == 0)
-		return (1);
-
-	/* Update header */
-	hdr.channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
-	hdr.msg_header = req->protocol_id << SMT_HEADER_PROTOCOL_ID_S;
-	hdr.msg_header |= req->message_id << SMT_HEADER_MESSAGE_ID_S;
+	req->msg_header = req->message_id << SCMI_HDR_MESSAGE_ID_S;
 	/* TODO: Allocate a token */
-	hdr.length = sizeof(hdr.msg_header) + req->in_size;
-	hdr.flags |= SCMI_SHMEM_FLAG_INTR_ENABLED;
+	req->msg_header |= SCMI_MSG_TYPE_CMD << SCMI_HDR_MESSAGE_TYPE_S;
+	req->msg_header |= req->protocol_id << SCMI_HDR_PROTOCOL_ID_S;
 
-	/* Write header */
-	scmi_shmem_write(sc->tx_shmem, 0, &hdr, SMT_HEADER_SIZE);
-
-	/* Write request */
-	scmi_shmem_write(sc->tx_shmem, SMT_HEADER_SIZE, req->in_buf,
-	    req->in_size);
+	ret = scmi_shmem_prepare_msg(sc->a2p_dev, req);
+	if (ret != 0)
+		return (ret);
 
 	ret = SCMI_XFER_MSG(sc->dev);
 	if (ret != 0)
 		return (ret);
 
 	/* Read header. */
-	scmi_shmem_read(sc->tx_shmem, 0, &hdr, SMT_HEADER_SIZE);
+	ret = scmi_shmem_read_msg_header(sc->a2p_dev, &reply_header);
+	if (ret != 0)
+		return (ret);
 
-	/* Read response */
-	scmi_shmem_read(sc->tx_shmem, SMT_HEADER_SIZE, req->out_buf,
-	    req->out_size);
+	if (reply_header != req->msg_header)
+		return (EPROTO);
 
-	return (0);
+	return (scmi_shmem_read_msg_payload(sc->a2p_dev, req->out_buf,
+	    req->out_size));
 }
 
 int
@@ -155,9 +134,9 @@ scmi_attach(device_t dev)
 	if (node == -1)
 		return (ENXIO);
 
-	sc->tx_shmem = scmi_get_shmem(sc, 0);
-	if (sc->tx_shmem == NULL) {
-		device_printf(dev, "TX shmem dev not found.\n");
+	sc->a2p_dev = scmi_shmem_get(dev, node, SCMI_CHAN_A2P);
+	if (sc->a2p_dev == NULL) {
+		device_printf(dev, "A2P shmem dev not found.\n");
 		return (ENXIO);
 	}
 
diff --git a/sys/dev/firmware/arm/scmi.h b/sys/dev/firmware/arm/scmi.h
index f1c81a609894..bebebff50429 100644
--- a/sys/dev/firmware/arm/scmi.h
+++ b/sys/dev/firmware/arm/scmi.h
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: BSD-2-Clause
  *
  * Copyright (c) 2022 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2023 Arm Ltd
  *
  * This work was supported by Innovate UK project 105694, "Digital Security
  * by Design (DSbD) Technology Platform Prototype".
@@ -39,41 +40,25 @@
 
 #define dprintf(fmt, ...)
 
+#define SCMI_MSG_HDR_SIZE	(sizeof(uint32_t))
+
+enum scmi_chan {
+	SCMI_CHAN_A2P,
+	SCMI_CHAN_P2A,
+	SCMI_CHAN_MAX
+};
+
 struct scmi_softc {
 	struct simplebus_softc	simplebus_sc;
 	device_t		dev;
-	device_t		tx_shmem;
+	device_t		a2p_dev;
 	struct mtx		mtx;
 };
 
-/* Shared Memory Transfer. */
-struct scmi_smt_header {
-	uint32_t reserved;
-	uint32_t channel_status;
-#define	SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR	(1 << 1)
-#define	SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE	(1 << 0)
-	uint32_t reserved1[2];
-	uint32_t flags;
-#define	SCMI_SHMEM_FLAG_INTR_ENABLED		(1 << 0)
-	uint32_t length;
-	uint32_t msg_header;
-	uint8_t msg_payload[0];
-};
-
-#define	SMT_HEADER_SIZE			sizeof(struct scmi_smt_header)
-
-#define	SMT_HEADER_TOKEN_S		18
-#define	SMT_HEADER_TOKEN_M		(0x3fff << SMT_HEADER_TOKEN_S)
-#define	SMT_HEADER_PROTOCOL_ID_S	10
-#define	SMT_HEADER_PROTOCOL_ID_M	(0xff << SMT_HEADER_PROTOCOL_ID_S)
-#define	SMT_HEADER_MESSAGE_TYPE_S	8
-#define	SMT_HEADER_MESSAGE_TYPE_M	(0x3 << SMT_HEADER_MESSAGE_TYPE_S)
-#define	SMT_HEADER_MESSAGE_ID_S		0
-#define	SMT_HEADER_MESSAGE_ID_M		(0xff << SMT_HEADER_MESSAGE_ID_S)
-
 struct scmi_req {
 	int protocol_id;
 	int message_id;
+	uint32_t msg_header;
 	const void *in_buf;
 	uint32_t in_size;
 	void *out_buf;
@@ -85,9 +70,4 @@ DECLARE_CLASS(scmi_driver);
 int scmi_attach(device_t dev);
 int scmi_request(device_t dev, struct scmi_req *req);
 
-void scmi_shmem_read(device_t dev, bus_size_t offset, void *buf,
-    bus_size_t len);
-void scmi_shmem_write(device_t dev, bus_size_t offset, const void *buf,
-    bus_size_t len);
-
 #endif /* !_ARM64_SCMI_SCMI_H_ */
diff --git a/sys/dev/firmware/arm/scmi_shmem.c b/sys/dev/firmware/arm/scmi_shmem.c
index 45a4414a7fb4..efb6b77c6e4b 100644
--- a/sys/dev/firmware/arm/scmi_shmem.c
+++ b/sys/dev/firmware/arm/scmi_shmem.c
@@ -2,6 +2,7 @@
  * SPDX-License-Identifier: BSD-2-Clause
  *
  * Copyright (c) 2022 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2023 Arm Ltd
  *
  * This work was supported by Innovate UK project 105694, "Digital Security
  * by Design (DSbD) Technology Platform Prototype".
@@ -41,6 +42,7 @@
 
 #include "mmio_sram_if.h"
 
+#include "scmi_shmem.h"
 #include "scmi.h"
 
 struct shmem_softc {
@@ -49,6 +51,14 @@ struct shmem_softc {
 	int			reg;
 };
 
+static void	scmi_shmem_read(device_t, bus_size_t, void *, bus_size_t);
+static void	scmi_shmem_write(device_t, bus_size_t, const void *,
+				 bus_size_t);
+
+static int	shmem_probe(device_t);
+static int	shmem_attach(device_t);
+static int	shmem_detach(device_t);
+
 static int
 shmem_probe(device_t dev)
 {
@@ -97,7 +107,7 @@ shmem_detach(device_t dev)
 	return (0);
 }
 
-void
+static void
 scmi_shmem_read(device_t dev, bus_size_t offset, void *buf, bus_size_t len)
 {
 	struct shmem_softc *sc;
@@ -112,7 +122,7 @@ scmi_shmem_read(device_t dev, bus_size_t offset, void *buf, bus_size_t len)
 		addr[i] = MMIO_SRAM_READ_1(sc->parent, sc->reg + offset + i);
 }
 
-void
+static void
 scmi_shmem_write(device_t dev, bus_size_t offset, const void *buf,
     bus_size_t len)
 {
@@ -128,6 +138,107 @@ scmi_shmem_write(device_t dev, bus_size_t offset, const void *buf,
 		MMIO_SRAM_WRITE_1(sc->parent, sc->reg + offset + i, addr[i]);
 }
 
+device_t
+scmi_shmem_get(device_t dev, phandle_t node, int index)
+{
+	phandle_t *shmems;
+	device_t shmem_dev;
+	size_t len;
+
+	len = OF_getencprop_alloc_multi(node, "shmem", sizeof(*shmems),
+	    (void **)&shmems);
+	if (len <= 0) {
+		device_printf(dev, "%s: Can't get shmem node.\n", __func__);
+		return (NULL);
+	}
+
+	if (index >= len) {
+		OF_prop_free(shmems);
+		return (NULL);
+	}
+
+	shmem_dev = OF_device_from_xref(shmems[index]);
+	if (shmem_dev == NULL)
+		device_printf(dev, "%s: Can't get shmem device.\n",
+		    __func__);
+
+	OF_prop_free(shmems);
+
+	return (shmem_dev);
+}
+
+int
+scmi_shmem_prepare_msg(device_t dev, struct scmi_req *req)
+{
+	struct scmi_smt_header hdr = {};
+	uint32_t channel_status;
+
+	/* Read channel status */
+	scmi_shmem_read(dev, SMT_OFFSET_CHAN_STATUS, &channel_status,
+	    SMT_SIZE_CHAN_STATUS);
+	if ((channel_status & SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE) == 0) {
+		device_printf(dev, "Shmem channel busy. Abort !.\n");
+		return (1);
+	}
+
+	/* Update header */
+	hdr.channel_status &= ~SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE;
+	hdr.msg_header = htole32(req->msg_header);
+	hdr.length = htole32(sizeof(req->msg_header) + req->in_size);
+	hdr.flags |= SCMI_SHMEM_FLAG_INTR_ENABLED;
+
+	/* Write header */
+	scmi_shmem_write(dev, 0, &hdr, SMT_SIZE_HEADER);
+
+	/* Write request payload if any */
+	if (req->in_size)
+		scmi_shmem_write(dev, SMT_SIZE_HEADER, req->in_buf,
+		    req->in_size);
+
+	return (0);
+}
+
+int
+scmi_shmem_read_msg_header(device_t dev, uint32_t *msg_header)
+{
+	uint32_t length, header;
+
+	/* Read and check length. */
+	scmi_shmem_read(dev, SMT_OFFSET_LENGTH, &length, SMT_SIZE_LENGTH);
+	if (le32toh(length) < sizeof(header))
+		return (EINVAL);
+
+	/* Read header. */
+	scmi_shmem_read(dev, SMT_OFFSET_MSG_HEADER, &header,
+	    SMT_SIZE_MSG_HEADER);
+
+	*msg_header = le32toh(header);
+
+	return (0);
+}
+
+int
+scmi_shmem_read_msg_payload(device_t dev, uint8_t *buf, uint32_t buf_len)
+{
+	uint32_t length, payld_len;
+
+	/* Read length. */
+	scmi_shmem_read(dev, SMT_OFFSET_LENGTH, &length, SMT_SIZE_LENGTH);
+	payld_len = le32toh(length) - SCMI_MSG_HDR_SIZE;
+
+	if (payld_len > buf_len) {
+		device_printf(dev,
+		    "RX payload %dbytes exceeds buflen %dbytes. Truncate.\n",
+		    payld_len, buf_len);
+		payld_len = buf_len;
+	}
+
+	/* Read response payload */
+	scmi_shmem_read(dev, SMT_SIZE_HEADER, buf, payld_len);
+
+	return (0);
+}
+
 static device_method_t shmem_methods[] = {
 	DEVMETHOD(device_probe,		shmem_probe),
 	DEVMETHOD(device_attach,	shmem_attach),
diff --git a/sys/dev/firmware/arm/scmi_shmem.h b/sys/dev/firmware/arm/scmi_shmem.h
new file mode 100644
index 000000000000..ce82a3c90d87
--- /dev/null
+++ b/sys/dev/firmware/arm/scmi_shmem.h
@@ -0,0 +1,70 @@
+/*-
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ * Copyright (c) 2022 Ruslan Bukin <br@bsdpad.com>
+ * Copyright (c) 2023 Arm Ltd
+ *
+ * This work was supported by Innovate UK project 105694, "Digital Security
+ * by Design (DSbD) Technology Platform Prototype".
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#ifndef	_ARM64_SCMI_SCMI_SHMEM_H_
+#define	_ARM64_SCMI_SCMI_SHMEM_H_
+
+/* Shared Memory Transfer. */
+struct scmi_smt_header {
+	uint32_t reserved;
+	uint32_t channel_status;
+#define	SCMI_SHMEM_CHAN_STAT_CHANNEL_ERROR	(1 << 1)
+#define	SCMI_SHMEM_CHAN_STAT_CHANNEL_FREE	(1 << 0)
+	uint32_t reserved1[2];
+	uint32_t flags;
+#define	SCMI_SHMEM_FLAG_INTR_ENABLED		(1 << 0)
+	uint32_t length;
+	uint32_t msg_header;
+	uint8_t msg_payload[0];
+};
+
+#define	SMT_SIZE_HEADER			sizeof(struct scmi_smt_header)
+
+#define	SMT_OFFSET_CHAN_STATUS		\
+	__offsetof(struct scmi_smt_header, channel_status)
+#define	SMT_SIZE_CHAN_STATUS		sizeof(uint32_t)
+
+#define	SMT_OFFSET_LENGTH		\
+	__offsetof(struct scmi_smt_header, length)
+#define	SMT_SIZE_LENGTH			sizeof(uint32_t)
+
+#define	SMT_OFFSET_MSG_HEADER		\
+    __offsetof(struct scmi_smt_header, msg_header)
+#define	SMT_SIZE_MSG_HEADER		sizeof(uint32_t)
+
+struct scmi_req;
+
+device_t	scmi_shmem_get(device_t sdev, phandle_t node, int index);
+int		scmi_shmem_prepare_msg(device_t dev, struct scmi_req *req);
+int scmi_shmem_read_msg_header(device_t dev, uint32_t *msg_header);
+int scmi_shmem_read_msg_payload(device_t dev, uint8_t *buf, uint32_t buf_len);
+
+#endif /* !_ARM64_SCMI_SCMI_SHMEM_H_ */