1 /*
2 * Copyright (c) 2018, NXP
3 * Copyright (c) 2018, Nordic Semiconductor ASA
4 * Copyright (c) 2018-2019, Linaro Limited
5 *
6 * SPDX-License-Identifier: Apache-2.0
7 */
8
9 #include <zephyr/kernel.h>
10 #include <zephyr/drivers/ipm.h>
11 #include <zephyr/sys/printk.h>
12 #include <zephyr/device.h>
13 #include <stdio.h>
14 #include <stdlib.h>
15 #include <string.h>
16 #include <zephyr/init.h>
17
18 #include <openamp/open_amp.h>
19
20 #include "common.h"
21
22 #define APP_TASK_STACK_SIZE (1024)
23 K_THREAD_STACK_DEFINE(thread_stack, APP_TASK_STACK_SIZE);
24 static struct k_thread thread_data;
25
26 static const struct device *const ipm_handle =
27 DEVICE_DT_GET(DT_CHOSEN(zephyr_ipc));
28
29 static metal_phys_addr_t shm_physmap[] = { SHM_START_ADDR };
30
31 static volatile unsigned int received_data;
32
33 static struct virtio_vring_info rvrings[2] = {
34 [0] = {
35 .info.align = VRING_ALIGNMENT,
36 },
37 [1] = {
38 .info.align = VRING_ALIGNMENT,
39 },
40 };
41 static struct virtio_device vdev;
42 static struct rpmsg_virtio_device rvdev;
43 static struct metal_io_region shm_io_data;
44 static struct metal_io_region *io = &shm_io_data;
45 static struct virtqueue *vqueue[2];
46
ipc_virtio_get_status(struct virtio_device * dev)47 static unsigned char ipc_virtio_get_status(struct virtio_device *dev)
48 {
49 return VIRTIO_CONFIG_STATUS_DRIVER_OK;
50 }
51
ipc_virtio_set_status(struct virtio_device * dev,unsigned char status)52 static void ipc_virtio_set_status(struct virtio_device *dev, unsigned char status)
53 {
54 sys_write8(status, VDEV_STATUS_ADDR);
55 }
56
ipc_virtio_get_features(struct virtio_device * dev)57 static uint32_t ipc_virtio_get_features(struct virtio_device *dev)
58 {
59 return 1 << VIRTIO_RPMSG_F_NS;
60 }
61
ipc_virtio_set_features(struct virtio_device * dev,uint32_t features)62 static void ipc_virtio_set_features(struct virtio_device *dev, uint32_t features)
63 {
64 }
65
ipc_virtio_notify(struct virtqueue * vq)66 static void ipc_virtio_notify(struct virtqueue *vq)
67 {
68 #if defined(CONFIG_SOC_MPS2_AN521) || \
69 defined(CONFIG_SOC_V2M_MUSCA_B1)
70 uint32_t current_core = sse_200_platform_get_cpu_id();
71
72 ipm_send(ipm_handle, 0, current_core ? 0 : 1, 0, 1);
73 #else
74 uint32_t dummy_data = 0x55005500; /* Some data must be provided */
75
76 ipm_send(ipm_handle, 0, 0, &dummy_data, sizeof(dummy_data));
77 #endif /* #if defined(CONFIG_SOC_MPS2_AN521) */
78 }
79
80 struct virtio_dispatch dispatch = {
81 .get_status = ipc_virtio_get_status,
82 .set_status = ipc_virtio_set_status,
83 .get_features = ipc_virtio_get_features,
84 .set_features = ipc_virtio_set_features,
85 .notify = ipc_virtio_notify,
86 };
87
88 static K_SEM_DEFINE(data_sem, 0, 1);
89 static K_SEM_DEFINE(data_rx_sem, 0, 1);
90
platform_ipm_callback(const struct device * dev,void * context,uint32_t id,volatile void * data)91 static void platform_ipm_callback(const struct device *dev, void *context,
92 uint32_t id, volatile void *data)
93 {
94 k_sem_give(&data_sem);
95 }
96
endpoint_cb(struct rpmsg_endpoint * ept,void * data,size_t len,uint32_t src,void * priv)97 int endpoint_cb(struct rpmsg_endpoint *ept, void *data,
98 size_t len, uint32_t src, void *priv)
99 {
100 received_data = *((unsigned int *) data);
101
102 k_sem_give(&data_rx_sem);
103
104 return RPMSG_SUCCESS;
105 }
106
107 static K_SEM_DEFINE(ept_sem, 0, 1);
108
109 struct rpmsg_endpoint my_ept;
110 struct rpmsg_endpoint *ep = &my_ept;
111
rpmsg_service_unbind(struct rpmsg_endpoint * ept)112 static void rpmsg_service_unbind(struct rpmsg_endpoint *ept)
113 {
114 (void)ept;
115 rpmsg_destroy_ept(ep);
116 }
117
ns_bind_cb(struct rpmsg_device * rdev,const char * name,uint32_t dest)118 void ns_bind_cb(struct rpmsg_device *rdev, const char *name, uint32_t dest)
119 {
120 (void)rpmsg_create_ept(ep, rdev, name,
121 RPMSG_ADDR_ANY, dest,
122 endpoint_cb,
123 rpmsg_service_unbind);
124
125 k_sem_give(&ept_sem);
126 }
127
receive_message(void)128 static unsigned int receive_message(void)
129 {
130 while (k_sem_take(&data_rx_sem, K_NO_WAIT) != 0) {
131 int status = k_sem_take(&data_sem, K_FOREVER);
132
133 if (status == 0) {
134 virtqueue_notification(vqueue[0]);
135 }
136 }
137 return received_data;
138 }
139
send_message(unsigned int message)140 static int send_message(unsigned int message)
141 {
142 return rpmsg_send(ep, &message, sizeof(message));
143 }
144
145 static struct rpmsg_virtio_shm_pool shpool;
146
app_task(void * arg1,void * arg2,void * arg3)147 void app_task(void *arg1, void *arg2, void *arg3)
148 {
149 ARG_UNUSED(arg1);
150 ARG_UNUSED(arg2);
151 ARG_UNUSED(arg3);
152
153 int status = 0;
154 unsigned int message = 0U;
155 struct metal_init_params metal_params = METAL_INIT_DEFAULTS;
156
157 printk("\r\nOpenAMP[master] demo started\r\n");
158
159 status = metal_init(&metal_params);
160 if (status != 0) {
161 printk("metal_init: failed - error code %d\n", status);
162 return;
163 }
164
165 /* declare shared memory region */
166 metal_io_init(io, (void *)SHM_START_ADDR, shm_physmap, SHM_SIZE, -1, 0, NULL);
167
168 /* setup IPM */
169 if (!device_is_ready(ipm_handle)) {
170 printk("IPM device is not ready\n");
171 return;
172 }
173
174 ipm_register_callback(ipm_handle, platform_ipm_callback, NULL);
175
176 status = ipm_set_enabled(ipm_handle, 1);
177 if (status != 0) {
178 printk("ipm_set_enabled failed\n");
179 return;
180 }
181
182 /* setup vdev */
183 vqueue[0] = virtqueue_allocate(VRING_SIZE);
184 if (vqueue[0] == NULL) {
185 printk("virtqueue_allocate failed to alloc vqueue[0]\n");
186 return;
187 }
188 vqueue[1] = virtqueue_allocate(VRING_SIZE);
189 if (vqueue[1] == NULL) {
190 printk("virtqueue_allocate failed to alloc vqueue[1]\n");
191 return;
192 }
193
194 vdev.role = RPMSG_HOST;
195 vdev.vrings_num = VRING_COUNT;
196 vdev.func = &dispatch;
197 rvrings[0].io = io;
198 rvrings[0].info.vaddr = (void *)VRING_TX_ADDRESS;
199 rvrings[0].info.num_descs = VRING_SIZE;
200 rvrings[0].info.align = VRING_ALIGNMENT;
201 rvrings[0].vq = vqueue[0];
202
203 rvrings[1].io = io;
204 rvrings[1].info.vaddr = (void *)VRING_RX_ADDRESS;
205 rvrings[1].info.num_descs = VRING_SIZE;
206 rvrings[1].info.align = VRING_ALIGNMENT;
207 rvrings[1].vq = vqueue[1];
208
209 vdev.vrings_info = &rvrings[0];
210
211 /* setup rvdev */
212 rpmsg_virtio_init_shm_pool(&shpool, (void *)SHM_START_ADDR, SHM_SIZE);
213 status = rpmsg_init_vdev(&rvdev, &vdev, ns_bind_cb, io, &shpool);
214 if (status != 0) {
215 printk("rpmsg_init_vdev failed %d\n", status);
216 return;
217 }
218
219 /* Since we are using name service, we need to wait for a response
220 * from NS setup and than we need to process it
221 */
222 k_sem_take(&data_sem, K_FOREVER);
223 virtqueue_notification(vqueue[0]);
224
225 /* Wait til nameservice ep is setup */
226 k_sem_take(&ept_sem, K_FOREVER);
227
228 while (message < 100) {
229 status = send_message(message);
230 if (status < 0) {
231 printk("send_message(%d) failed with status %d\n",
232 message, status);
233 goto _cleanup;
234 }
235
236 message = receive_message();
237 printk("Master core received a message: %d\n", message);
238
239 message++;
240 }
241
242 _cleanup:
243 rpmsg_deinit_vdev(&rvdev);
244 metal_finish();
245
246 printk("OpenAMP demo ended.\n");
247 }
248
main(void)249 int main(void)
250 {
251 printk("Starting application thread!\n");
252 k_thread_create(&thread_data, thread_stack, APP_TASK_STACK_SIZE,
253 app_task,
254 NULL, NULL, NULL, K_PRIO_COOP(7), 0, K_NO_WAIT);
255
256 #if defined(CONFIG_SOC_MPS2_AN521) || \
257 defined(CONFIG_SOC_V2M_MUSCA_B1)
258 wakeup_cpu1();
259 k_msleep(500);
260 #endif /* #if defined(CONFIG_SOC_MPS2_AN521) */
261 return 0;
262 }
263
264 /* Make sure we clear out the status flag very early (before we bringup the
265 * secondary core) so the secondary core see's the proper status
266 */
init_status_flag(void)267 int init_status_flag(void)
268 {
269 ipc_virtio_set_status(NULL, 0);
270
271 return 0;
272 }
273
274 SYS_INIT(init_status_flag, PRE_KERNEL_1, CONFIG_KERNEL_INIT_PRIORITY_DEFAULT);
275
