Add openamp messenger
Create an openamp messenger that will handle the setup and
communication to an outside PE using openamp and using mhu as a doorbell
mechanism.
Signed-off-by: Vishnu Banavath <vishnu.banavath@arm.com>
Signed-off-by: Rui Miguel Silva <rui.silva@linaro.org>
Change-Id: Iac4119faac3f98dc04e7edbc1f401db8c8c93537
diff --git a/components/messaging/openamp/sp/component.cmake b/components/messaging/openamp/sp/component.cmake
new file mode 100644
index 0000000..cee76a0
--- /dev/null
+++ b/components/messaging/openamp/sp/component.cmake
@@ -0,0 +1,24 @@
+#-------------------------------------------------------------------------------
+# Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+#-------------------------------------------------------------------------------
+if (NOT DEFINED TGT)
+ message(FATAL_ERROR "mandatory parameter TGT is not defined.")
+endif()
+
+set_property(TARGET ${TGT} APPEND PROPERTY PUBLIC_HEADER
+ "${CMAKE_CURRENT_LIST_DIR}/openamp_messenger_api.h"
+ )
+
+target_sources(${TGT} PRIVATE
+ "${CMAKE_CURRENT_LIST_DIR}/openamp_messenger.c"
+ "${CMAKE_CURRENT_LIST_DIR}/openamp_virtio.c"
+ "${CMAKE_CURRENT_LIST_DIR}/openamp_mhu.c"
+ )
+
+target_include_directories(${TGT}
+ PUBLIC
+ "${CMAKE_CURRENT_LIST_DIR}"
+ )
diff --git a/components/messaging/openamp/sp/openamp_messenger.c b/components/messaging/openamp/sp/openamp_messenger.c
new file mode 100644
index 0000000..2b77ea4
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_messenger.c
@@ -0,0 +1,166 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <errno.h>
+#include <stddef.h>
+#include <trace.h>
+#include "openamp_messenger.h"
+#include "openamp_messenger_api.h"
+#include "openamp_mhu.h"
+#include "openamp_virtio.h"
+#include <protocols/rpc/common/packed-c/status.h>
+
+#define OPENAMP_TRANSACTION_IDLE 0x0
+#define OPENAMP_TRANSACTION_INPROGRESS 0x1
+#define OPENAMP_TRANSACTION_INVOKED 0x2
+
+int openamp_messenger_call_begin(struct openamp_messenger *openamp,
+ uint8_t **req_buf, size_t req_len)
+{
+ const struct openamp_platform_ops *ops = openamp->platform_ops;
+ int ret;
+
+ if (!openamp)
+ return -EINVAL;
+
+ ops = openamp->platform_ops;
+ if (!req_buf) {
+ EMSG("openamp: call_begin: not req_buf");
+ return -EINVAL;
+ }
+
+ if (req_len > UINT32_MAX || req_len == 0) {
+ EMSG("openamp: call_begin: resp_len invalid: %lu", req_len);
+ return -EINVAL;
+ }
+
+ if (openamp->status != OPENAMP_TRANSACTION_IDLE) {
+ EMSG("openamp: call_begin: transaction not idle");
+ return -EINVAL;
+ }
+
+ ret = ops->platform_call_begin(openamp, req_buf, req_len);
+ if (ret < 0) {
+ EMSG("openamp: call_begin: platform begin failed: %d", ret);
+ return -EINVAL;
+ }
+
+ openamp->status = OPENAMP_TRANSACTION_INPROGRESS;
+
+ return 0;
+}
+
+int openamp_messenger_call_invoke(struct openamp_messenger *openamp,
+ uint8_t **resp_buf, size_t *resp_len)
+{
+ const struct openamp_platform_ops *ops;
+ int ret;
+
+ if (!openamp || !resp_buf || !resp_len) {
+ EMSG("openamp: call_invoke: invalid arguments");
+ return -EINVAL;
+ }
+
+ if (openamp->status != OPENAMP_TRANSACTION_INPROGRESS) {
+ EMSG("openamp: call_invoke: transaction needed to be started");
+ return -ENOTCONN;
+ }
+
+ ops = openamp->platform_ops;
+
+ ret = ops->platform_call_invoke(openamp, resp_buf, resp_len);
+ if (ret < 0)
+ return ret;
+
+ openamp->status = OPENAMP_TRANSACTION_INVOKED;
+
+ return 0;
+}
+
+void openamp_messenger_call_end(struct openamp_messenger *openamp)
+{
+ const struct openamp_platform_ops *ops;
+
+ if (!openamp)
+ return;
+
+ if (openamp->status == OPENAMP_TRANSACTION_IDLE) {
+ EMSG("openamp: call_end: transaction idle");
+ return;
+ }
+
+ ops = openamp->platform_ops;
+
+ ops->platform_call_end(openamp);
+
+ openamp->status = OPENAMP_TRANSACTION_IDLE;
+}
+
+void *openamp_messenger_phys_to_virt(struct openamp_messenger *openamp,
+ void *pa)
+{
+ const struct openamp_platform_ops *ops = openamp->platform_ops;
+
+ return ops->platform_phys_to_virt(openamp, pa);
+}
+
+void *openamp_messenger_virt_to_phys(struct openamp_messenger *openamp,
+ void *va)
+{
+ const struct openamp_platform_ops *ops = openamp->platform_ops;
+
+ return ops->platform_virt_to_phys(openamp, va);
+}
+
+static const struct openamp_platform_ops openamp_virtio_ops = {
+ .transport_init = openamp_mhu_init,
+ .transport_deinit = openamp_mhu_deinit,
+ .transport_notify = openamp_mhu_notify_peer,
+ .transport_receive = openamp_mhu_receive,
+ .platform_init = openamp_virtio_init,
+ .platform_call_begin = openamp_virtio_call_begin,
+ .platform_call_invoke = openamp_virtio_call_invoke,
+ .platform_call_end = openamp_virtio_call_end,
+ .platform_virt_to_phys = openamp_virtio_virt_to_phys,
+ .platform_phys_to_virt = openamp_virtio_phys_to_virt,
+};
+
+int openamp_messenger_init(struct openamp_messenger *openamp)
+{
+ const struct openamp_platform_ops *ops;
+ int ret;
+
+ if (openamp->ref_count)
+ return 0;
+
+ openamp->platform_ops = &openamp_virtio_ops;
+
+ ops = openamp->platform_ops;
+
+ ret = ops->transport_init(openamp);
+ if (ret < 0)
+ return ret;
+
+ ret = ops->platform_init(openamp);
+ if (ret < 0)
+ goto denit_transport;
+
+ openamp->ref_count++;
+
+ return 0;
+
+denit_transport:
+ ops->transport_deinit(openamp);
+
+ return ret;
+}
+
+void openamp_messenger_deinit(struct openamp_messenger *openamp)
+{
+ if (--openamp->ref_count)
+ return;
+}
diff --git a/components/messaging/openamp/sp/openamp_messenger.h b/components/messaging/openamp/sp/openamp_messenger.h
new file mode 100644
index 0000000..e863c4f
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_messenger.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef OPENAMP_MESSENGER_H
+#define OPENAMP_MESSENGER_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+struct openamp_messenger;
+
+struct openamp_platform_ops {
+ int (*transport_init)(struct openamp_messenger *openamp);
+ int (*transport_deinit)(struct openamp_messenger *openamp);
+ int (*transport_notify)(struct openamp_messenger *openamp);
+ int (*transport_receive)(struct openamp_messenger *openamp);
+ int (*platform_init)(struct openamp_messenger *openamp);
+ int (*platform_deinit)(struct openamp_messenger *openamp);
+ int (*platform_call_begin)(struct openamp_messenger *openamp,
+ uint8_t **req_buf, size_t req_len);
+ int (*platform_call_invoke)(struct openamp_messenger *openamp,
+ uint8_t **resp_buf, size_t *resp_len);
+ int (*platform_call_end)(struct openamp_messenger *openamp);
+ void *(*platform_virt_to_phys)(struct openamp_messenger *openamp,
+ void *va);
+ void *(*platform_phys_to_virt)(struct openamp_messenger *openamp,
+ void *pa);
+};
+
+#endif
diff --git a/components/messaging/openamp/sp/openamp_messenger_api.h b/components/messaging/openamp/sp/openamp_messenger_api.h
new file mode 100644
index 0000000..61ab37a
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_messenger_api.h
@@ -0,0 +1,36 @@
+/*
+ * Copyright (c) 2022-2023, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2022-2023, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef OPENAMP_MESSENGER_API_H
+#define OPENAMP_MESSENGER_API_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include "openamp_messenger.h"
+
+struct openamp_messenger {
+ const struct openamp_platform_ops *platform_ops;
+ uint32_t ref_count;
+ uint8_t status;
+
+ void *transport;
+ void *platform;
+};
+
+int openamp_messenger_init(struct openamp_messenger *openamp);
+void openamp_messenger_deinit(struct openamp_messenger *openamp);
+void openamp_messenger_call_end(struct openamp_messenger *openamp);
+int openamp_messenger_call_invoke(struct openamp_messenger *openamp,
+ uint8_t **resp_buf, size_t *resp_len);
+int openamp_messenger_call_begin(struct openamp_messenger *openamp,
+ uint8_t **req_buf, size_t req_len);
+
+void *openamp_messenger_phys_to_virt(struct openamp_messenger *openamp,
+ void *pa);
+void *openamp_messenger_virt_to_phys(struct openamp_messenger *openamp,
+ void *va);
+#endif
diff --git a/components/messaging/openamp/sp/openamp_mhu.c b/components/messaging/openamp/sp/openamp_mhu.c
new file mode 100644
index 0000000..bafba3e
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_mhu.c
@@ -0,0 +1,191 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <config/interface/config_store.h>
+#include <config/interface/config_blob.h>
+#include <platform/interface/device_region.h>
+#include <platform/drivers/arm/mhu_driver/mhu_v2.h>
+#include <trace.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <stdint.h>
+#include <stddef.h>
+#include <limits.h>
+
+#include "openamp_messenger_api.h"
+
+#define MHU_V_2_NOTIFY_CHANNEL 0
+#define MHU_V_2_NOTIFY_VALUE 0xff
+
+struct openamp_mhu {
+ struct device_region rx_region;
+ struct device_region tx_region;
+ struct mhu_v2_x_dev_t rx_dev;
+ struct mhu_v2_x_dev_t tx_dev;
+};
+
+static int openamp_mhu_device_get(const char *dev,
+ struct device_region *dev_region)
+{
+ bool found;
+
+ found = config_store_query(CONFIG_CLASSIFIER_DEVICE_REGION, dev, 0,
+ dev_region, sizeof(*dev_region));
+ if (!found)
+ return -EINVAL;
+
+ if (!dev_region->base_addr)
+ return -EINVAL;
+
+ IMSG("mhu: device region found: %s addr: 0x%p size: %lu", dev,
+ (void *)dev_region->base_addr, dev_region->io_region_size);
+
+ return 0;
+}
+
+int openamp_mhu_receive(struct openamp_messenger *openamp)
+{
+ struct mhu_v2_x_dev_t *rx_dev;
+ enum mhu_v2_x_error_t ret;
+ struct openamp_mhu *mhu;
+ uint32_t channel = 0;
+ uint32_t irq_status;
+
+ if (!openamp->transport) {
+ EMSG("openamp: mhu: receive transport not initialized");
+ return -EINVAL;
+ }
+
+ mhu = openamp->transport;
+ rx_dev = &mhu->rx_dev;
+
+ irq_status = 0;
+
+ do {
+ irq_status = mhu_v2_x_get_interrupt_status(rx_dev);
+ } while(!irq_status);
+
+ ret = mhu_v2_1_get_ch_interrupt_num(rx_dev, &channel);
+
+ ret = mhu_v2_x_channel_clear(rx_dev, channel);
+ if (ret != MHU_V_2_X_ERR_NONE) {
+ EMSG("openamp: mhu: failed to clear channel: %d", channel);
+ return -EPROTO;
+ }
+
+ return 0;
+}
+
+int openamp_mhu_notify_peer(struct openamp_messenger *openamp)
+{
+ struct mhu_v2_x_dev_t *tx_dev;
+ enum mhu_v2_x_error_t ret;
+ struct openamp_mhu *mhu;
+ uint32_t access_ready;
+
+ if (!openamp->transport) {
+ EMSG("openamp: mhu: notify transport not initialized");
+ return -EINVAL;
+ }
+
+ mhu = openamp->transport;
+ tx_dev = &mhu->tx_dev;
+
+ ret = mhu_v2_x_set_access_request(tx_dev);
+ if (ret != MHU_V_2_X_ERR_NONE) {
+ EMSG("openamp: mhu: set access request failed");
+ return -EPROTO;
+ }
+
+ do {
+ ret = mhu_v2_x_get_access_ready(tx_dev, &access_ready);
+ if (ret != MHU_V_2_X_ERR_NONE) {
+ EMSG("openamp: mhu: failed to get access_ready");
+ return -EPROTO;
+ }
+ } while (!access_ready);
+
+ ret = mhu_v2_x_channel_send(tx_dev, MHU_V_2_NOTIFY_CHANNEL,
+ MHU_V_2_NOTIFY_VALUE);
+ if (ret != MHU_V_2_X_ERR_NONE) {
+ EMSG("openamp: mhu: failed send over channel");
+ return -EPROTO;
+ }
+
+ ret = mhu_v2_x_reset_access_request(tx_dev);
+ if (ret != MHU_V_2_X_ERR_NONE) {
+ EMSG("openamp: mhu: failed reset access request");
+ return -EPROTO;
+ }
+
+ return 0;
+}
+
+int openamp_mhu_init(struct openamp_messenger *openamp)
+{
+ struct mhu_v2_x_dev_t *rx_dev;
+ struct mhu_v2_x_dev_t *tx_dev;
+ struct openamp_mhu *mhu;
+ int ret;
+
+ /* if we already have initialized skip this */
+ if (openamp->transport)
+ return 0;
+
+ mhu = malloc(sizeof(*mhu));
+ if (!mhu)
+ return -1;
+
+ ret = openamp_mhu_device_get("mhu-sender", &mhu->tx_region);
+ if (ret < 0)
+ goto free_mhu;
+
+ ret = openamp_mhu_device_get("mhu-receiver", &mhu->rx_region);
+ if (ret < 0)
+ goto free_mhu;
+
+ rx_dev = &mhu->rx_dev;
+ tx_dev = &mhu->tx_dev;
+
+ rx_dev->base = mhu->rx_region.base_addr;
+ rx_dev->frame = MHU_V2_X_RECEIVER_FRAME;
+
+ tx_dev->base = mhu->tx_region.base_addr;
+ tx_dev->frame = MHU_V2_X_SENDER_FRAME;
+
+ ret = mhu_v2_x_driver_init(rx_dev, MHU_REV_READ_FROM_HW);
+ if (ret < 0)
+ goto free_mhu;
+
+ ret = mhu_v2_x_driver_init(tx_dev, MHU_REV_READ_FROM_HW);
+ if (ret < 0)
+ goto free_mhu;
+
+ openamp->transport = (void *)mhu;
+
+ return 0;
+
+free_mhu:
+ free(mhu);
+
+ return ret;
+}
+
+int openamp_mhu_deinit(struct openamp_messenger *openamp)
+{
+ struct openamp_mhu *mhu;
+
+ if (!openamp->transport)
+ return 0;
+
+ mhu = openamp->transport;
+ free(mhu);
+
+ openamp->transport = NULL;
+
+ return 0;
+}
diff --git a/components/messaging/openamp/sp/openamp_mhu.h b/components/messaging/openamp/sp/openamp_mhu.h
new file mode 100644
index 0000000..4ba427a
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_mhu.h
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef OPENAMP_MHU_H
+#define OPENAMP_MHU_H
+
+#include <stddef.h>
+#include "openamp_messenger_api.h"
+
+int openamp_mhu_init(struct openamp_messenger *openamp);
+int openamp_mhu_deinit(struct openamp_messenger *openamp);
+
+int openamp_mhu_notify_peer(struct openamp_messenger *openamp);
+int openamp_mhu_receive(struct openamp_messenger *openamp);
+
+#endif
diff --git a/components/messaging/openamp/sp/openamp_virtio.c b/components/messaging/openamp/sp/openamp_virtio.c
new file mode 100644
index 0000000..71d1ceb
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_virtio.c
@@ -0,0 +1,552 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <metal/device.h>
+#include <metal/spinlock.h>
+#include <openamp/open_amp.h>
+#include <platform/interface/device_region.h>
+#include <config/interface/config_store.h>
+
+#include <errno.h>
+#include <stddef.h>
+#include <trace.h>
+#include "openamp_messenger_api.h"
+
+#define OPENAMP_SHEM_DEVICE_NAME "openamp-virtio"
+#define OPENAMP_RPMSG_ENDPOINT_NAME OPENAMP_SHEM_DEVICE_NAME
+#define OPENAMP_RPMSG_ENDPOINT_ADDR 1024
+
+#define OPENAMP_SHEM_PHYS 0x88000000
+#define OPENAMP_SHEM_PHYS_PAGES 1
+#define OPENAMP_SHEM_SE_PHYS 0xa8000000
+
+#define OPENAMP_SHEM_VDEV_SIZE (4 * 1024)
+#define OPENAMP_SHEM_VRING_SIZE (4 * 1024)
+
+#define OPENAMP_BUFFER_NO_WAIT 0
+#define OPENAMP_BUFFER_WAIT 1
+
+#define VIRTQUEUE_NR 2
+#define VQ_TX 0
+#define VQ_RX 1
+
+#define VRING_DESCRIPTORS 16
+#define VRING_ALIGN 4
+
+#define container_of(ptr, type, member) \
+ ((type *)((char *)(ptr) - (unsigned long)(&((type *)0)->member)))
+
+struct openamp_virtio_shm {
+ uintptr_t base_addr;
+ size_t size;
+ uintptr_t vdev_status;
+ size_t vdev_status_size;
+ uintptr_t payload_addr;
+ size_t payload_size;
+ uintptr_t vring_tx;
+ size_t vring_tx_size;
+ uintptr_t vring_rx;
+ size_t vring_rx_size;
+
+ metal_phys_addr_t shm_physmap[OPENAMP_SHEM_PHYS_PAGES];
+};
+
+struct openamp_virtio_metal {
+ struct metal_spinlock lock;
+ struct metal_device shm_dev;
+ struct metal_device *io_dev;
+
+ struct metal_io_region *io;
+ struct openamp_virtio_shm shm;
+};
+
+struct openamp_virtio_device {
+ struct virtio_device virtio_dev;
+ struct virtqueue *vq[VIRTQUEUE_NR];
+ struct virtio_vring_info rvrings[VIRTQUEUE_NR];
+};
+
+struct openamp_virtio_rpmsg {
+ struct rpmsg_virtio_device rpmsg_vdev;
+ struct rpmsg_endpoint ep;
+ uint8_t *req_buf;
+ uint32_t req_len;
+ uint8_t *resp_buf;
+ size_t resp_len;
+};
+
+struct openamp_virtio {
+ struct openamp_messenger *openamp;
+ struct openamp_virtio_rpmsg rpmsg;
+ struct openamp_virtio_device vdev;
+ struct openamp_virtio_metal metal;
+};
+
+static struct openamp_virtio *openamp_virtio_from_dev(struct virtio_device *vdev)
+{
+ struct openamp_virtio_device *openamp_vdev;
+
+ openamp_vdev = container_of(vdev, struct openamp_virtio_device,
+ virtio_dev);
+
+ return container_of(openamp_vdev, struct openamp_virtio, vdev);
+}
+
+static struct openamp_virtio_rpmsg *openamp_virtio_rpmsg_from_dev(struct rpmsg_device *rdev)
+{
+ struct rpmsg_virtio_device *rvdev;
+
+ rvdev = container_of(rdev, struct rpmsg_virtio_device, rdev);
+
+ return container_of(rvdev, struct openamp_virtio_rpmsg, rpmsg_vdev);
+
+}
+
+static void openamp_virtio_metal_device_setup(struct metal_device *shm_dev,
+ struct openamp_virtio_shm *shm)
+{
+ struct metal_io_region *shm_region;
+
+ shm_region = &shm_dev->regions[0];
+
+ shm_dev->name = OPENAMP_SHEM_DEVICE_NAME;
+ shm_dev->num_regions = 1;
+
+ shm_region->virt = (void *)shm->payload_addr;
+ shm_region->size = shm->payload_size;
+
+ shm_region->physmap = (metal_phys_addr_t *)&shm->shm_physmap;
+ shm_region->page_shift = (metal_phys_addr_t)(-1);
+ shm_region->page_mask = (metal_phys_addr_t)(-1);
+}
+
+static int openamp_virtio_metal_init(struct openamp_virtio_metal *metal)
+{
+ struct metal_init_params params = METAL_INIT_DEFAULTS;
+ struct metal_device *shm_dev = &metal->shm_dev;
+ int ret;
+
+ openamp_virtio_metal_device_setup(shm_dev, &metal->shm);
+
+ metal_spinlock_init(&metal->lock);
+
+ ret = metal_init(¶ms);
+ if (ret < 0)
+ return ret;
+
+ ret = metal_register_generic_device(shm_dev);
+ if (ret < 0)
+ goto metal_finish;
+
+ ret = metal_device_open("generic", OPENAMP_SHEM_DEVICE_NAME,
+ &metal->io_dev);
+ if (ret < 0)
+ goto metal_finish;
+
+ metal->io = metal_device_io_region(metal->io_dev, 0);
+ if (!metal->io) {
+ EMSG("openamp: virtio: failed to init metal io");
+ ret = -EPROTO;
+ goto metal_finish;
+ }
+
+ return 0;
+
+metal_finish:
+ metal_finish();
+ return ret;
+}
+
+static unsigned char openamp_virtio_status_get(struct virtio_device *vdev)
+{
+ struct openamp_virtio *virtio = openamp_virtio_from_dev(vdev);
+ struct openamp_virtio_shm *shm = &virtio->metal.shm;
+
+ uint32_t status = *(volatile uint32_t *)shm->vdev_status;
+
+ return status;
+}
+
+static void openamp_virtio_status_set(struct virtio_device *vdev,
+ unsigned char status)
+{
+ struct openamp_virtio *virtio = openamp_virtio_from_dev(vdev);
+ struct openamp_virtio_shm *shm = &virtio->metal.shm;
+
+ *(volatile uint32_t *)shm->vdev_status = status;
+}
+
+static uint32_t openamp_virtio_features_get(struct virtio_device *vdev)
+{
+ return 1 << VIRTIO_RPMSG_F_NS;
+}
+
+static void openamp_virtio_notify(struct virtqueue *vq)
+{
+ struct openamp_virtio_device *openamp_vdev;
+ struct openamp_messenger *openamp;
+ struct openamp_virtio *virtio;
+ int ret;
+
+ openamp_vdev = container_of(vq->vq_dev, struct openamp_virtio_device, virtio_dev);
+ virtio = container_of(openamp_vdev, struct openamp_virtio, vdev);
+ openamp = virtio->openamp;
+
+ ret = openamp->platform_ops->transport_notify(openamp);
+ if (ret < 0)
+ EMSG("openamp: virtio: erro in transport_notify: %d", ret);
+}
+
+const static struct virtio_dispatch openamp_virtio_dispatch = {
+ .get_status = openamp_virtio_status_get,
+ .set_status = openamp_virtio_status_set,
+ .get_features = openamp_virtio_features_get,
+ .notify = openamp_virtio_notify,
+};
+
+static int openamp_virtio_device_setup(struct openamp_virtio *virtio)
+{
+ struct openamp_virtio_metal *metal = &virtio->metal;
+ struct openamp_virtio_device *openamp_vdev = &virtio->vdev;
+ struct virtio_device *vdev = &openamp_vdev->virtio_dev;
+ struct openamp_virtio_shm *shm = &metal->shm;
+ struct virtio_vring_info *rvring;
+
+ rvring = &openamp_vdev->rvrings[0];
+
+ vdev->role = RPMSG_REMOTE;
+ vdev->vrings_num = VIRTQUEUE_NR;
+ vdev->func = &openamp_virtio_dispatch;
+
+ openamp_vdev->vq[VQ_TX] = virtqueue_allocate(VRING_DESCRIPTORS);
+ if (!openamp_vdev->vq[VQ_TX]) {
+ EMSG("openamp: virtio: failed to allocate virtqueue 0");
+ return -ENOMEM;
+ }
+ rvring->io = metal->io;
+ rvring->info.vaddr = (void *)shm->vring_tx;
+ rvring->info.num_descs = VRING_DESCRIPTORS;
+ rvring->info.align = VRING_ALIGN;
+ rvring->vq = openamp_vdev->vq[VQ_TX];
+
+ openamp_vdev->vq[VQ_RX] = virtqueue_allocate(VRING_DESCRIPTORS);
+ if (!openamp_vdev->vq[VQ_RX]) {
+ EMSG("openamp: virtio: failed to allocate virtqueue 1");
+ goto free_vq;
+ }
+ rvring = &openamp_vdev->rvrings[VQ_RX];
+ rvring->io = metal->io;
+ rvring->info.vaddr = (void *)shm->vring_rx;
+ rvring->info.num_descs = VRING_DESCRIPTORS;
+ rvring->info.align = VRING_ALIGN;
+ rvring->vq = openamp_vdev->vq[VQ_RX];
+
+ vdev->vrings_info = &openamp_vdev->rvrings[0];
+
+ return 0;
+
+free_vq:
+ virtqueue_free(openamp_vdev->vq[VQ_TX]);
+ virtqueue_free(openamp_vdev->vq[VQ_RX]);
+
+ return -ENOMEM;
+}
+
+static int openamp_virtio_rpmsg_endpoint_callback(struct rpmsg_endpoint *ep,
+ void *data, size_t len,
+ uint32_t src, void *priv)
+{
+ struct openamp_virtio_rpmsg *vrpmsg;
+ struct rpmsg_device *rdev;
+
+ rdev = ep->rdev;
+ vrpmsg = openamp_virtio_rpmsg_from_dev(rdev);
+
+ rpmsg_hold_rx_buffer(ep, data);
+ vrpmsg->resp_buf = data;
+ vrpmsg->resp_len = len;
+
+ return 0;
+}
+
+static void openamp_virtio_rpmsg_service_unbind(struct rpmsg_endpoint *ep)
+{
+ struct openamp_virtio_rpmsg *vrpmsg;
+ struct rpmsg_device *rdev;
+
+ rdev = container_of(ep, struct rpmsg_device, ns_ept);
+ vrpmsg = openamp_virtio_rpmsg_from_dev(rdev);
+
+ rpmsg_destroy_ept(&vrpmsg->ep);
+}
+
+static void openamp_virtio_rpmsg_endpoint_bind(struct rpmsg_device *rdev,
+ const char *name,
+ unsigned int dest)
+{
+ struct openamp_virtio_rpmsg *vrpmsg;
+
+ vrpmsg = openamp_virtio_rpmsg_from_dev(rdev);
+
+ rpmsg_create_ept(&vrpmsg->ep, rdev, name, RPMSG_ADDR_ANY, dest,
+ openamp_virtio_rpmsg_endpoint_callback,
+ openamp_virtio_rpmsg_service_unbind);
+}
+
+static int openamp_virtio_rpmsg_device_setup(struct openamp_virtio *virtio,
+ struct device_region *virtio_dev)
+{
+ struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
+ struct rpmsg_virtio_device *rpmsg_vdev = &vrpmsg->rpmsg_vdev;
+ struct openamp_virtio_device *openamp_vdev = &virtio->vdev;
+ struct virtio_device *vdev = &openamp_vdev->virtio_dev;
+ struct openamp_virtio_metal *metal = &virtio->metal;
+ int ret;
+
+ /*
+ * we assume here that we are the client side and do not need to
+ * initialize the share memory poll (this is done at server side).
+ */
+ ret = rpmsg_init_vdev(rpmsg_vdev, vdev,
+ openamp_virtio_rpmsg_endpoint_bind, metal->io,
+ NULL);
+ if (ret < 0) {
+ EMSG("openamp: virtio: init vdev failed: %d", ret);
+ return ret;
+ }
+
+
+ ret = rpmsg_create_ept(&vrpmsg->ep, &rpmsg_vdev->rdev,
+ OPENAMP_RPMSG_ENDPOINT_NAME, RPMSG_ADDR_ANY,
+ RPMSG_ADDR_ANY,
+ openamp_virtio_rpmsg_endpoint_callback,
+ openamp_virtio_rpmsg_service_unbind);
+ if (ret < 0) {
+ EMSG("openamp: virtio: failed to create endpoint: %d", ret);
+ return ret;
+ }
+
+ /* set default remote addr */
+ vrpmsg->ep.dest_addr = OPENAMP_RPMSG_ENDPOINT_ADDR;
+
+ return 0;
+}
+
+static void openamp_virtio_shm_set(struct openamp_virtio *virtio,
+ struct device_region *virtio_region)
+{
+ struct openamp_virtio_shm *shm = &virtio->metal.shm;
+
+ shm->base_addr = virtio_region->base_addr;
+ shm->size = virtio_region->io_region_size;
+
+ shm->vdev_status = shm->base_addr;
+ shm->vdev_status_size = OPENAMP_SHEM_VDEV_SIZE;
+
+ shm->vring_rx = shm->base_addr + shm->size -
+ (2 * OPENAMP_SHEM_VRING_SIZE);
+ shm->vring_rx_size = OPENAMP_SHEM_VRING_SIZE;
+
+ shm->vring_tx = shm->vring_rx + shm->vring_rx_size;
+ shm->vring_tx_size = OPENAMP_SHEM_VRING_SIZE;
+
+ shm->payload_addr = shm->vdev_status + shm->vdev_status_size;
+ shm->payload_size = shm->size - shm->vdev_status_size -
+ shm->vring_rx_size - shm->vring_tx_size;
+
+ shm->shm_physmap[0] = OPENAMP_SHEM_PHYS + shm->vdev_status_size;
+
+ IMSG("SHEM: base: 0x%p size: %ld",
+ (void *)shm->base_addr, shm->size);
+ IMSG("VDEV: base: 0x%p size: %ld",
+ (void *)shm->vdev_status, shm->vdev_status_size);
+ IMSG("PAYLOAD: base: 0x%p size: %ld",
+ (void *)shm->payload_addr, shm->payload_size);
+ IMSG("VRING_TX: base: 0x%p size: %ld",
+ (void *)shm->vring_tx, shm->vring_tx_size);
+ IMSG("VRING_RX: base: 0x%p size: %ld",
+ (void *)shm->vring_rx, shm->vring_rx_size);
+ IMSG("PHYMAP: base: 0x%p", (void *)shm->shm_physmap[0]);
+}
+
+static int openamp_virtio_device_get(const char *dev,
+ struct device_region *dev_region)
+{
+ bool found;
+
+ found = config_store_query(CONFIG_CLASSIFIER_DEVICE_REGION, dev, 0,
+ dev_region, sizeof(*dev_region));
+ if (!found) {
+ EMSG("openamp: virtio: device region not found: %s", dev);
+ return -EINVAL;
+ }
+
+ if (dev_region->base_addr == 0 || dev_region->io_region_size == 0) {
+ EMSG("openamp: virtio: device region not valid");
+ return -EINVAL;
+ }
+
+ IMSG("openamp: virtio: device region found: %s addr: 0x%p size: %ld",
+ dev, (void *)dev_region->base_addr, dev_region->io_region_size);
+
+ return 0;
+}
+
+int openamp_virtio_call_begin(struct openamp_messenger *openamp, uint8_t **req_buf,
+ size_t req_len)
+{
+ struct openamp_virtio *virtio = openamp->platform;
+ struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
+ struct rpmsg_endpoint *ep = &vrpmsg->ep;
+
+
+ *req_buf = rpmsg_get_tx_payload_buffer(ep, &vrpmsg->req_len,
+ OPENAMP_BUFFER_WAIT);
+ if (*req_buf == NULL)
+ return -EINVAL;
+
+ if (vrpmsg->req_len < req_len)
+ return -E2BIG;
+
+ vrpmsg->req_buf = *req_buf;
+
+ return 0;
+}
+
+int openamp_virtio_call_invoke(struct openamp_messenger *openamp,
+ uint8_t **resp_buf, size_t *resp_len)
+{
+ const struct openamp_platform_ops *ops = openamp->platform_ops;
+ struct openamp_virtio *virtio = openamp->platform;
+ struct openamp_virtio_device *openamp_vdev = &virtio->vdev;
+ struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
+ struct rpmsg_endpoint *ep = &vrpmsg->ep;
+ int ret;
+
+ ret = rpmsg_send_nocopy(ep, vrpmsg->req_buf, vrpmsg->req_len);
+ if (ret < 0) {
+ EMSG("openamp: virtio: send nocopy failed: %d", ret);
+ return -EIO;
+ }
+
+ if (ret != vrpmsg->req_len) {
+ EMSG("openamp: virtio: send less bytes %d than requested %d",
+ ret, vrpmsg->req_len);
+ return -EIO;
+ }
+
+ if (!ops->transport_receive)
+ return 0;
+
+ ret = ops->transport_receive(openamp);
+ if (ret < 0) {
+ EMSG("openamp: virtio: failed transport_receive");
+ return -EIO;
+ }
+
+ virtqueue_notification(openamp_vdev->vq[VQ_RX]);
+
+ *resp_buf = vrpmsg->resp_buf;
+ *resp_len = vrpmsg->resp_len;
+
+ return 0;
+}
+
+void openamp_virtio_call_end(struct openamp_messenger *openamp)
+{
+ struct openamp_virtio *virtio = openamp->platform;
+ struct openamp_virtio_rpmsg *vrpmsg = &virtio->rpmsg;
+
+ rpmsg_release_rx_buffer(&vrpmsg->ep, vrpmsg->resp_buf);
+
+ vrpmsg->req_buf = NULL;
+ vrpmsg->req_len = 0;
+ vrpmsg->resp_buf = NULL;
+ vrpmsg->resp_len = 0;
+}
+
+void *openamp_virtio_virt_to_phys(struct openamp_messenger *openamp, void *va)
+{
+ struct openamp_virtio *virtio = openamp->platform;
+ struct openamp_virtio_metal *metal = &virtio->metal;
+
+ return (void *)metal_io_virt_to_phys(metal->io, va);
+}
+
+void *openamp_virtio_phys_to_virt(struct openamp_messenger *openamp, void *pa)
+{
+ struct openamp_virtio *virtio = openamp->platform;
+ struct openamp_virtio_metal *metal = &virtio->metal;
+
+ return metal_io_phys_to_virt(metal->io, (metal_phys_addr_t)pa);
+}
+
+int openamp_virtio_init(struct openamp_messenger *openamp)
+{
+ struct device_region virtio_dev;
+ struct openamp_virtio *virtio;
+ int ret;
+
+ if (openamp->platform)
+ return 0;
+
+
+ virtio = malloc(sizeof(*virtio));
+ if (!virtio)
+ return -ENOMEM;
+
+ virtio->openamp = openamp;
+
+ ret = openamp_virtio_device_get(OPENAMP_SHEM_DEVICE_NAME, &virtio_dev);
+ if (ret < 0)
+ goto free_virtio;
+
+ openamp_virtio_shm_set(virtio, &virtio_dev);
+
+ ret = openamp_virtio_metal_init(&virtio->metal);
+ if (ret < 0)
+ goto free_virtio;
+
+ ret = openamp_virtio_device_setup(virtio);
+ if (ret < 0)
+ goto finish_metal;
+
+ ret = openamp_virtio_rpmsg_device_setup(virtio, &virtio_dev);
+ if (ret < 0) {
+ EMSG("openamp: virtio: rpmsg device setup failed: %d", ret);
+ goto finish_metal;
+ }
+
+ openamp->platform = virtio;
+
+ return 0;
+
+finish_metal:
+ metal_finish();
+
+free_virtio:
+ free(virtio);
+
+ return ret;
+}
+
+int openamp_virtio_deinit(struct openamp_messenger *openamp)
+{
+ struct openamp_virtio *virtio;
+
+ if (!openamp->platform)
+ return 0;
+
+ virtio = openamp->platform;
+
+ metal_finish();
+ free(virtio);
+
+ openamp->platform = NULL;
+
+ return 0;
+}
diff --git a/components/messaging/openamp/sp/openamp_virtio.h b/components/messaging/openamp/sp/openamp_virtio.h
new file mode 100644
index 0000000..bb4f7a5
--- /dev/null
+++ b/components/messaging/openamp/sp/openamp_virtio.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2021-2023, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2023, Linaro Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef OPENAMP_VIRTIO_H
+#define OPENAMP_VIRTIO_H
+
+#include <stddef.h>
+#include "openamp_messenger_api.h"
+
+int openamp_virtio_call_begin(struct openamp_messenger *openamp,
+ uint8_t **req_buf, size_t req_len);
+int openamp_virtio_call_invoke(struct openamp_messenger *openamp,
+ uint8_t **resp_buf, size_t *resp_len);
+int openamp_virtio_call_end(struct openamp_messenger *openamp);
+void *openamp_virtio_virt_to_phys(struct openamp_messenger *openamp, void *va);
+void *openamp_virtio_phys_to_virt(struct openamp_messenger *openamp, void *pa);
+
+int openamp_virtio_init(struct openamp_messenger *openamp);
+int openamp_virtio_deinit(struct openamp_messenger *openamp);
+
+#endif