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