1 /*
2 * Copyright (c) 2020, STMICROELECTRONICS
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/kernel.h>
8 #include <zephyr/device.h>
9 #include <string.h>
10 #include <stdio.h>
11 #include <stdlib.h>
12 #include <string.h>
13
14 #include <zephyr/drivers/ipm.h>
15
16 #include <openamp/open_amp.h>
17 #include <metal/sys.h>
18 #include <metal/io.h>
19 #include <resource_table.h>
20
21 #ifdef CONFIG_SHELL_BACKEND_RPMSG
22 #include <zephyr/shell/shell_rpmsg.h>
23 #endif
24
25 #include <zephyr/logging/log.h>
26 LOG_MODULE_REGISTER(openamp_rsc_table, LOG_LEVEL_DBG);
27
28 #define SHM_DEVICE_NAME "shm"
29
30 #if !DT_HAS_CHOSEN(zephyr_ipc_shm)
31 #error "Sample requires definition of shared memory for rpmsg"
32 #endif
33
34 /* Constants derived from device tree */
35 #define SHM_NODE DT_CHOSEN(zephyr_ipc_shm)
36 #define SHM_START_ADDR DT_REG_ADDR(SHM_NODE)
37 #define SHM_SIZE DT_REG_SIZE(SHM_NODE)
38
39 #define APP_TASK_STACK_SIZE (1024)
40
41 /* Add 1024 extra bytes for the TTY task stack for the "tx_buff" buffer. */
42 #define APP_TTY_TASK_STACK_SIZE (1536)
43
44 K_THREAD_STACK_DEFINE(thread_mng_stack, APP_TASK_STACK_SIZE);
45 K_THREAD_STACK_DEFINE(thread_rp__client_stack, APP_TASK_STACK_SIZE);
46 K_THREAD_STACK_DEFINE(thread_tty_stack, APP_TTY_TASK_STACK_SIZE);
47
48 static struct k_thread thread_mng_data;
49 static struct k_thread thread_rp__client_data;
50 static struct k_thread thread_tty_data;
51
52 static const struct device *const ipm_handle =
53 DEVICE_DT_GET(DT_CHOSEN(zephyr_ipc));
54
55 static metal_phys_addr_t shm_physmap = SHM_START_ADDR;
56 static metal_phys_addr_t rsc_tab_physmap;
57
58 static struct metal_io_region shm_io_data; /* shared memory */
59 static struct metal_io_region rsc_io_data; /* rsc_table memory */
60
61 struct rpmsg_rcv_msg {
62 void *data;
63 size_t len;
64 };
65
66 static struct metal_io_region *shm_io = &shm_io_data;
67
68 static struct metal_io_region *rsc_io = &rsc_io_data;
69 static struct rpmsg_virtio_device rvdev;
70
71 static struct fw_resource_table *rsc_table;
72 static struct rpmsg_device *rpdev;
73
74 static char rx_sc_msg[20]; /* should receive "Hello world!" */
75 static struct rpmsg_endpoint sc_ept;
76 static struct rpmsg_rcv_msg sc_msg = {.data = rx_sc_msg};
77
78 static struct rpmsg_endpoint tty_ept;
79 static struct rpmsg_rcv_msg tty_msg;
80
81 static K_SEM_DEFINE(data_sem, 0, 1);
82 static K_SEM_DEFINE(data_sc_sem, 0, 1);
83 static K_SEM_DEFINE(data_tty_sem, 0, 1);
84
platform_ipm_callback(const struct device * dev,void * context,uint32_t id,volatile void * data)85 static void platform_ipm_callback(const struct device *dev, void *context,
86 uint32_t id, volatile void *data)
87 {
88 LOG_DBG("%s: msg received from mb %d", __func__, id);
89 k_sem_give(&data_sem);
90 }
91
rpmsg_recv_cs_callback(struct rpmsg_endpoint * ept,void * data,size_t len,uint32_t src,void * priv)92 static int rpmsg_recv_cs_callback(struct rpmsg_endpoint *ept, void *data,
93 size_t len, uint32_t src, void *priv)
94 {
95 memcpy(sc_msg.data, data, len);
96 sc_msg.len = len;
97 k_sem_give(&data_sc_sem);
98
99 return RPMSG_SUCCESS;
100 }
101
rpmsg_recv_tty_callback(struct rpmsg_endpoint * ept,void * data,size_t len,uint32_t src,void * priv)102 static int rpmsg_recv_tty_callback(struct rpmsg_endpoint *ept, void *data,
103 size_t len, uint32_t src, void *priv)
104 {
105 struct rpmsg_rcv_msg *msg = priv;
106
107 rpmsg_hold_rx_buffer(ept, data);
108 msg->data = data;
109 msg->len = len;
110 k_sem_give(&data_tty_sem);
111
112 return RPMSG_SUCCESS;
113 }
114
receive_message(unsigned char ** msg,unsigned int * len)115 static void receive_message(unsigned char **msg, unsigned int *len)
116 {
117 int status = k_sem_take(&data_sem, K_FOREVER);
118
119 if (status == 0) {
120 rproc_virtio_notified(rvdev.vdev, VRING1_ID);
121 }
122 }
123
new_service_cb(struct rpmsg_device * rdev,const char * name,uint32_t src)124 static void new_service_cb(struct rpmsg_device *rdev, const char *name,
125 uint32_t src)
126 {
127 LOG_ERR("%s: unexpected ns service receive for name %s",
128 __func__, name);
129 }
130
mailbox_notify(void * priv,uint32_t id)131 int mailbox_notify(void *priv, uint32_t id)
132 {
133 ARG_UNUSED(priv);
134
135 LOG_DBG("%s: msg received", __func__);
136 ipm_send(ipm_handle, 0, id, NULL, 0);
137
138 return 0;
139 }
140
platform_init(void)141 int platform_init(void)
142 {
143 int rsc_size;
144 struct metal_init_params metal_params = METAL_INIT_DEFAULTS;
145 int status;
146
147 status = metal_init(&metal_params);
148 if (status) {
149 LOG_ERR("metal_init: failed: %d", status);
150 return -1;
151 }
152
153 /* declare shared memory region */
154 metal_io_init(shm_io, (void *)SHM_START_ADDR, &shm_physmap,
155 SHM_SIZE, -1, 0, NULL);
156
157 /* declare resource table region */
158 rsc_table_get(&rsc_table, &rsc_size);
159 rsc_tab_physmap = (uintptr_t)rsc_table;
160
161 metal_io_init(rsc_io, rsc_table,
162 &rsc_tab_physmap, rsc_size, -1, 0, NULL);
163
164 /* setup IPM */
165 if (!device_is_ready(ipm_handle)) {
166 LOG_ERR("IPM device is not ready");
167 return -1;
168 }
169
170 ipm_register_callback(ipm_handle, platform_ipm_callback, NULL);
171
172 status = ipm_set_enabled(ipm_handle, 1);
173 if (status) {
174 LOG_ERR("ipm_set_enabled failed");
175 return -1;
176 }
177
178 return 0;
179 }
180
cleanup_system(void)181 static void cleanup_system(void)
182 {
183 ipm_set_enabled(ipm_handle, 0);
184 rpmsg_deinit_vdev(&rvdev);
185 metal_finish();
186 }
187
188 struct rpmsg_device *
platform_create_rpmsg_vdev(unsigned int vdev_index,unsigned int role,void (* rst_cb)(struct virtio_device * vdev),rpmsg_ns_bind_cb ns_cb)189 platform_create_rpmsg_vdev(unsigned int vdev_index,
190 unsigned int role,
191 void (*rst_cb)(struct virtio_device *vdev),
192 rpmsg_ns_bind_cb ns_cb)
193 {
194 struct fw_rsc_vdev_vring *vring_rsc;
195 struct virtio_device *vdev;
196 int ret;
197
198 vdev = rproc_virtio_create_vdev(VIRTIO_DEV_DEVICE, VDEV_ID,
199 rsc_table_to_vdev(rsc_table),
200 rsc_io, NULL, mailbox_notify, NULL);
201
202 if (!vdev) {
203 LOG_ERR("failed to create vdev");
204 return NULL;
205 }
206
207 /* wait master rpmsg init completion */
208 rproc_virtio_wait_remote_ready(vdev);
209
210 vring_rsc = rsc_table_get_vring0(rsc_table);
211 ret = rproc_virtio_init_vring(vdev, 0, vring_rsc->notifyid,
212 (void *)vring_rsc->da, rsc_io,
213 vring_rsc->num, vring_rsc->align);
214 if (ret) {
215 LOG_ERR("failed to init vring 0");
216 goto failed;
217 }
218
219 vring_rsc = rsc_table_get_vring1(rsc_table);
220 ret = rproc_virtio_init_vring(vdev, 1, vring_rsc->notifyid,
221 (void *)vring_rsc->da, rsc_io,
222 vring_rsc->num, vring_rsc->align);
223 if (ret) {
224 LOG_ERR("failed to init vring 1");
225 goto failed;
226 }
227
228 ret = rpmsg_init_vdev(&rvdev, vdev, ns_cb, shm_io, NULL);
229 if (ret) {
230 LOG_ERR("failed rpmsg_init_vdev");
231 goto failed;
232 }
233
234 return rpmsg_virtio_get_rpmsg_device(&rvdev);
235
236 failed:
237 rproc_virtio_remove_vdev(vdev);
238
239 return NULL;
240 }
241
app_rpmsg_client_sample(void * arg1,void * arg2,void * arg3)242 void app_rpmsg_client_sample(void *arg1, void *arg2, void *arg3)
243 {
244 ARG_UNUSED(arg1);
245 ARG_UNUSED(arg2);
246 ARG_UNUSED(arg3);
247
248 unsigned int msg_cnt = 0;
249 int ret = 0;
250
251 k_sem_take(&data_sc_sem, K_FOREVER);
252
253 LOG_INF("OpenAMP[remote] Linux sample client responder started");
254
255 ret = rpmsg_create_ept(&sc_ept, rpdev, "rpmsg-client-sample",
256 RPMSG_ADDR_ANY, RPMSG_ADDR_ANY,
257 rpmsg_recv_cs_callback, NULL);
258 if (ret) {
259 LOG_ERR("[Linux sample client] Could not create endpoint: %d", ret);
260 goto task_end;
261 }
262
263 while (msg_cnt < 100) {
264 k_sem_take(&data_sc_sem, K_FOREVER);
265 msg_cnt++;
266 LOG_INF("[Linux sample client] incoming msg %d: %.*s", msg_cnt, sc_msg.len,
267 (char *)sc_msg.data);
268 rpmsg_send(&sc_ept, sc_msg.data, sc_msg.len);
269 }
270 rpmsg_destroy_ept(&sc_ept);
271
272 task_end:
273 LOG_INF("OpenAMP Linux sample client responder ended");
274 }
275
app_rpmsg_tty(void * arg1,void * arg2,void * arg3)276 void app_rpmsg_tty(void *arg1, void *arg2, void *arg3)
277 {
278 ARG_UNUSED(arg1);
279 ARG_UNUSED(arg2);
280 ARG_UNUSED(arg3);
281
282 unsigned char tx_buff[512];
283 int ret = 0;
284
285 k_sem_take(&data_tty_sem, K_FOREVER);
286
287 LOG_INF("OpenAMP[remote] Linux TTY responder started");
288
289 tty_ept.priv = &tty_msg;
290 ret = rpmsg_create_ept(&tty_ept, rpdev, "rpmsg-tty",
291 RPMSG_ADDR_ANY, RPMSG_ADDR_ANY,
292 rpmsg_recv_tty_callback, NULL);
293 if (ret) {
294 LOG_ERR("[Linux TTY] Could not create endpoint: %d", ret);
295 goto task_end;
296 }
297
298 while (tty_ept.addr != RPMSG_ADDR_ANY) {
299 k_sem_take(&data_tty_sem, K_FOREVER);
300 if (tty_msg.len) {
301 LOG_INF("[Linux TTY] incoming msg: %.*s",
302 (int)tty_msg.len, (char *)tty_msg.data);
303 snprintf(tx_buff, 13, "TTY 0x%04x: ", tty_ept.addr);
304 memcpy(&tx_buff[12], tty_msg.data, tty_msg.len);
305 rpmsg_send(&tty_ept, tx_buff, tty_msg.len + 12);
306 rpmsg_release_rx_buffer(&tty_ept, tty_msg.data);
307 }
308 tty_msg.len = 0;
309 tty_msg.data = NULL;
310 }
311 rpmsg_destroy_ept(&tty_ept);
312
313 task_end:
314 LOG_INF("OpenAMP Linux TTY responder ended");
315 }
316
rpmsg_mng_task(void * arg1,void * arg2,void * arg3)317 void rpmsg_mng_task(void *arg1, void *arg2, void *arg3)
318 {
319 ARG_UNUSED(arg1);
320 ARG_UNUSED(arg2);
321 ARG_UNUSED(arg3);
322
323 unsigned char *msg;
324 unsigned int len;
325 int ret = 0;
326
327 LOG_INF("OpenAMP[remote] Linux responder demo started");
328
329 /* Initialize platform */
330 ret = platform_init();
331 if (ret) {
332 LOG_ERR("Failed to initialize platform");
333 ret = -1;
334 goto task_end;
335 }
336
337 rpdev = platform_create_rpmsg_vdev(0, VIRTIO_DEV_DEVICE, NULL,
338 new_service_cb);
339 if (!rpdev) {
340 LOG_ERR("Failed to create rpmsg virtio device");
341 ret = -1;
342 goto task_end;
343 }
344
345 #ifdef CONFIG_SHELL_BACKEND_RPMSG
346 (void)shell_backend_rpmsg_init_transport(rpdev);
347 #endif
348
349 /* start the rpmsg clients */
350 k_sem_give(&data_sc_sem);
351 k_sem_give(&data_tty_sem);
352
353 while (1) {
354 receive_message(&msg, &len);
355 }
356
357 task_end:
358 cleanup_system();
359
360 LOG_INF("OpenAMP demo ended");
361 }
362
main(void)363 int main(void)
364 {
365 LOG_INF("Starting application threads!");
366 k_thread_create(&thread_mng_data, thread_mng_stack, APP_TASK_STACK_SIZE,
367 rpmsg_mng_task,
368 NULL, NULL, NULL, K_PRIO_COOP(8), 0, K_NO_WAIT);
369 k_thread_create(&thread_rp__client_data, thread_rp__client_stack, APP_TASK_STACK_SIZE,
370 app_rpmsg_client_sample,
371 NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
372 k_thread_create(&thread_tty_data, thread_tty_stack, APP_TTY_TASK_STACK_SIZE,
373 app_rpmsg_tty,
374 NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
375 return 0;
376 }
377
