|
| 1 | +/* |
| 2 | + * Copyright (c) 2020, STMICROELECTRONICS |
| 3 | + * Author: Thong Phan <[email protected]> |
| 4 | + * |
| 5 | + * Heavily based on pwm_mcux_ftm.c, which is: |
| 6 | + * Copyright (c) 2017, NXP |
| 7 | + * |
| 8 | + * SPDX-License-Identifier: Apache-2.0 |
| 9 | + */ |
| 10 | + |
| 11 | +#include "rpmsg_transport.h" |
| 12 | + |
| 13 | +#include <zephyr/logging/log.h> |
| 14 | +LOG_MODULE_REGISTER(rpmsg_transport); |
| 15 | +#include <zephyr/device.h> |
| 16 | +#include <zephyr/drivers/ipm.h> |
| 17 | + |
| 18 | +#include <metal/sys.h> |
| 19 | +#include <metal/io.h> |
| 20 | +#include <resource_table.h> |
| 21 | +#include <addr_translation.h> |
| 22 | + |
| 23 | +#if !DT_HAS_CHOSEN(zephyr_ipc_shm) |
| 24 | +#error "Sample requires definition of shared memory for rpmsg" |
| 25 | +#endif |
| 26 | + |
| 27 | +/* Constants from device tree */ |
| 28 | +#define SHM_NODE DT_CHOSEN(zephyr_ipc_shm) |
| 29 | +#define SHM_START_ADDR DT_REG_ADDR(SHM_NODE) |
| 30 | +#define SHM_SIZE DT_REG_SIZE(SHM_NODE) |
| 31 | +/* Stack size for the management thread */ |
| 32 | +#define APP_TASK_STACK_SIZE (1024) |
| 33 | + |
| 34 | +#if CONFIG_IPM_MAX_DATA_SIZE > 0 |
| 35 | +#define IPM_SEND(dev, w, id, d, s) ipm_send(dev, w, id, d, s) |
| 36 | +#else |
| 37 | +#define IPM_SEND(dev, w, id, d, s) ipm_send(dev, w, id, NULL, 0) |
| 38 | +#endif |
| 39 | +/* Thread definitions */ |
| 40 | +K_THREAD_STACK_DEFINE(thread_mng_stack, APP_TASK_STACK_SIZE); |
| 41 | +static struct k_thread thread_mng_data; |
| 42 | +/* OpenAMP components */ |
| 43 | +static const struct device *const ipm_handle = DEVICE_DT_GET(DT_CHOSEN(zephyr_ipc)); |
| 44 | +static metal_phys_addr_t shm_physmap = SHM_START_ADDR; |
| 45 | +static metal_phys_addr_t rsc_tab_physmap; |
| 46 | +static struct metal_io_region shm_io_data; |
| 47 | +static struct metal_io_region rsc_io_data; |
| 48 | +static struct metal_io_region *shm_io = &shm_io_data; |
| 49 | +static struct metal_io_region *rsc_io = &rsc_io_data; |
| 50 | +static struct rpmsg_virtio_device rvdev; |
| 51 | +static void *rsc_table; |
| 52 | +/* Public variables */ |
| 53 | +struct rpmsg_device *rpdev; |
| 54 | +struct rpmsg_endpoint tty_ept; |
| 55 | +K_MSGQ_DEFINE(tty_msgq, sizeof(struct rpmsg_rcv_msg), 16, 4); |
| 56 | +/* 16 messages * 20ms per frames covers 320ms of data */ |
| 57 | +K_SEM_DEFINE(data_tty_ready_sem, 0, 1); |
| 58 | +/* Internal semaphore, hidden from other files */ |
| 59 | +static K_SEM_DEFINE(data_sem, 0, 1); |
| 60 | + |
| 61 | +static void platform_ipm_callback(const struct device *dev, void *context, uint32_t id, |
| 62 | + volatile void *data) |
| 63 | + |
| 64 | +{ |
| 65 | + LOG_DBG("%s: msg received from mb %d", __func__, id); |
| 66 | + k_sem_give(&data_sem); |
| 67 | +} |
| 68 | + |
| 69 | +int rpmsg_recv_tty_callback(struct rpmsg_endpoint *ept, void *data, size_t len, uint32_t src, |
| 70 | + void *priv) |
| 71 | +{ |
| 72 | + struct rpmsg_rcv_msg msg = {.data = data, .len = len}; |
| 73 | + |
| 74 | + rpmsg_hold_rx_buffer(ept, data); |
| 75 | + |
| 76 | + if (k_msgq_put(&tty_msgq, &msg, K_NO_WAIT) != 0) { |
| 77 | + LOG_WRN("tty_msgq full, dropping frame len %zu\n", len); |
| 78 | + rpmsg_release_rx_buffer(ept, data); |
| 79 | + } |
| 80 | + return RPMSG_SUCCESS; |
| 81 | +} |
| 82 | + |
| 83 | +static void receive_message(unsigned char **msg, unsigned int *len) |
| 84 | +{ |
| 85 | + if (k_sem_take(&data_sem, K_FOREVER) == 0) { |
| 86 | + rproc_virtio_notified(rvdev.vdev, VRING1_ID); |
| 87 | + } |
| 88 | +} |
| 89 | + |
| 90 | +static void new_service_cb(struct rpmsg_device *rdev, const char *name, uint32_t src) |
| 91 | +{ |
| 92 | + LOG_ERR("%s: unexpected ns service receive for name %s", __func__, name); |
| 93 | +} |
| 94 | + |
| 95 | +static int mailbox_notify(void *priv, uint32_t id) |
| 96 | +{ |
| 97 | + ARG_UNUSED(priv); |
| 98 | + LOG_DBG("%s: msg received", __func__); |
| 99 | + IPM_SEND(ipm_handle, 0, id, &id, 4); |
| 100 | + return 0; |
| 101 | +} |
| 102 | + |
| 103 | +static int platform_init(void) |
| 104 | +{ |
| 105 | + int rsc_size; |
| 106 | + struct metal_init_params metal_params = METAL_INIT_DEFAULTS; |
| 107 | + int status; |
| 108 | + |
| 109 | + status = metal_init(&metal_params); |
| 110 | + if (status) { |
| 111 | + LOG_ERR("metal_init: failed: %d", status); |
| 112 | + return -1; |
| 113 | + } |
| 114 | + |
| 115 | + metal_io_init(shm_io, (void *)SHM_START_ADDR, &shm_physmap, SHM_SIZE, -1, 0, |
| 116 | + addr_translation_get_ops(shm_physmap)); |
| 117 | + |
| 118 | + rsc_table_get(&rsc_table, &rsc_size); |
| 119 | + rsc_tab_physmap = (uintptr_t)rsc_table; |
| 120 | + metal_io_init(rsc_io, rsc_table, &rsc_tab_physmap, rsc_size, -1, 0, NULL); |
| 121 | + |
| 122 | + if (!device_is_ready(ipm_handle)) { |
| 123 | + LOG_ERR("IPM device is not ready"); |
| 124 | + return -1; |
| 125 | + } |
| 126 | + |
| 127 | + ipm_register_callback(ipm_handle, platform_ipm_callback, NULL); |
| 128 | + |
| 129 | + status = ipm_set_enabled(ipm_handle, 1); |
| 130 | + if (status) { |
| 131 | + LOG_ERR("ipm_set_enabled failed"); |
| 132 | + return -1; |
| 133 | + } |
| 134 | + |
| 135 | + return 0; |
| 136 | +} |
| 137 | + |
| 138 | +static void cleanup_system(void) |
| 139 | +{ |
| 140 | + ipm_set_enabled(ipm_handle, 0); |
| 141 | + rpmsg_deinit_vdev(&rvdev); |
| 142 | + metal_finish(); |
| 143 | +} |
| 144 | + |
| 145 | +static struct rpmsg_device *platform_create_rpmsg_vdev(rpmsg_ns_bind_cb ns_cb) |
| 146 | +{ |
| 147 | + struct fw_rsc_vdev_vring *vring_rsc; |
| 148 | + struct virtio_device *vdev; |
| 149 | + int ret; |
| 150 | + |
| 151 | + vdev = rproc_virtio_create_vdev(VIRTIO_DEV_DEVICE, VDEV_ID, rsc_table_to_vdev(rsc_table), |
| 152 | + rsc_io, NULL, mailbox_notify, NULL); |
| 153 | + if (!vdev) { |
| 154 | + LOG_ERR("failed to create vdev"); |
| 155 | + return NULL; |
| 156 | + } |
| 157 | + |
| 158 | + rproc_virtio_wait_remote_ready(vdev); |
| 159 | + |
| 160 | + vring_rsc = rsc_table_get_vring0(rsc_table); |
| 161 | + ret = rproc_virtio_init_vring(vdev, 0, vring_rsc->notifyid, (void *)vring_rsc->da, rsc_io, |
| 162 | + vring_rsc->num, vring_rsc->align); |
| 163 | + if (ret) { |
| 164 | + LOG_ERR("failed to init vring 0"); |
| 165 | + goto failed; |
| 166 | + } |
| 167 | + |
| 168 | + vring_rsc = rsc_table_get_vring1(rsc_table); |
| 169 | + ret = rproc_virtio_init_vring(vdev, 1, vring_rsc->notifyid, (void *)vring_rsc->da, rsc_io, |
| 170 | + vring_rsc->num, vring_rsc->align); |
| 171 | + if (ret) { |
| 172 | + LOG_ERR("failed to init vring 1"); |
| 173 | + goto failed; |
| 174 | + } |
| 175 | + |
| 176 | + ret = rpmsg_init_vdev(&rvdev, vdev, ns_cb, shm_io, NULL); |
| 177 | + if (ret) { |
| 178 | + LOG_ERR("failed rpmsg_init_vdev"); |
| 179 | + goto failed; |
| 180 | + } |
| 181 | + |
| 182 | + return rpmsg_virtio_get_rpmsg_device(&rvdev); |
| 183 | + |
| 184 | +failed: |
| 185 | + rproc_virtio_remove_vdev(vdev); |
| 186 | + return NULL; |
| 187 | +} |
| 188 | + |
| 189 | +static void rpmsg_mng_task(void *arg1, void *arg2, void *arg3) |
| 190 | +{ |
| 191 | + ARG_UNUSED(arg1); |
| 192 | + ARG_UNUSED(arg2); |
| 193 | + ARG_UNUSED(arg3); |
| 194 | + |
| 195 | + unsigned char *msg; |
| 196 | + unsigned int len; |
| 197 | + int ret = 0; |
| 198 | + |
| 199 | + LOG_INF("Microspeech sample with OpenAMP started"); |
| 200 | + |
| 201 | + ret = platform_init(); |
| 202 | + if (ret) { |
| 203 | + LOG_ERR("Failed to initialize platform"); |
| 204 | + goto task_end; |
| 205 | + } |
| 206 | + |
| 207 | + rpdev = platform_create_rpmsg_vdev(new_service_cb); |
| 208 | + if (!rpdev) { |
| 209 | + LOG_ERR("Failed to create rpmsg virtio device"); |
| 210 | + goto task_end; |
| 211 | + } |
| 212 | + |
| 213 | + k_sem_give(&data_tty_ready_sem); |
| 214 | + |
| 215 | + while (1) { |
| 216 | + receive_message(&msg, &len); |
| 217 | + } |
| 218 | + |
| 219 | +task_end: |
| 220 | + cleanup_system(); |
| 221 | + LOG_INF("Microspeech sample with OpenAMP ended"); |
| 222 | +} |
| 223 | + |
| 224 | +/* Public Function */ |
| 225 | +void rpmsg_transport_start(void) |
| 226 | +{ |
| 227 | + k_thread_create(&thread_mng_data, thread_mng_stack, APP_TASK_STACK_SIZE, rpmsg_mng_task, |
| 228 | + NULL, NULL, NULL, K_PRIO_COOP(8), 0, K_NO_WAIT); |
| 229 | +} |
0 commit comments