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