1 /*
2 * Copyright (c) 2021 Carlo Caione <ccaione@baylibre.com>
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6
7 #include <zephyr/ipc/ipc_static_vrings.h>
8 #include <zephyr/cache.h>
9
10 #define SHM_DEVICE_NAME "sram0.shm"
11
12 #define RPMSG_VQ_0 (0) /* TX virtqueue queue index */
13 #define RPMSG_VQ_1 (1) /* RX virtqueue queue index */
14
ipc_virtio_notify(struct virtqueue * vq)15 static void ipc_virtio_notify(struct virtqueue *vq)
16 {
17 struct ipc_static_vrings *vr;
18
19 vr = CONTAINER_OF(vq->vq_dev, struct ipc_static_vrings, vdev);
20
21 if (vr->notify_cb) {
22 vr->notify_cb(vq, vr->priv);
23 }
24 }
25
ipc_virtio_set_features(struct virtio_device * vdev,uint32_t features)26 static void ipc_virtio_set_features(struct virtio_device *vdev, uint32_t features)
27 {
28 /* No need for implementation */
29 }
30
ipc_virtio_set_status(struct virtio_device * p_vdev,unsigned char status)31 static void ipc_virtio_set_status(struct virtio_device *p_vdev, unsigned char status)
32 {
33 struct ipc_static_vrings *vr;
34
35 if (p_vdev->role != VIRTIO_DEV_DRIVER) {
36 return;
37 }
38
39 vr = CONTAINER_OF(p_vdev, struct ipc_static_vrings, vdev);
40
41 sys_write8(status, vr->status_reg_addr);
42 sys_cache_data_flush_range((void *) vr->status_reg_addr, sizeof(status));
43 }
44
ipc_virtio_get_features(struct virtio_device * vdev)45 static uint32_t ipc_virtio_get_features(struct virtio_device *vdev)
46 {
47 return BIT(VIRTIO_RPMSG_F_NS);
48 }
49
ipc_virtio_get_status(struct virtio_device * p_vdev)50 static unsigned char ipc_virtio_get_status(struct virtio_device *p_vdev)
51 {
52 struct ipc_static_vrings *vr;
53 uint8_t ret;
54
55 vr = CONTAINER_OF(p_vdev, struct ipc_static_vrings, vdev);
56
57 ret = VIRTIO_CONFIG_STATUS_DRIVER_OK;
58
59 if (p_vdev->role == VIRTIO_DEV_DEVICE) {
60 sys_cache_data_invd_range((void *) vr->status_reg_addr, sizeof(ret));
61 ret = sys_read8(vr->status_reg_addr);
62 }
63
64 return ret;
65 }
66
67 const static struct virtio_dispatch dispatch = {
68 .get_status = ipc_virtio_get_status,
69 .get_features = ipc_virtio_get_features,
70 .set_status = ipc_virtio_set_status,
71 .set_features = ipc_virtio_set_features,
72 .notify = ipc_virtio_notify,
73 };
74
libmetal_setup(struct ipc_static_vrings * vr)75 static int libmetal_setup(struct ipc_static_vrings *vr)
76 {
77 struct metal_init_params metal_params = METAL_INIT_DEFAULTS;
78 struct metal_device *device;
79 int err;
80
81 err = metal_init(&metal_params);
82 if (err != 0) {
83 return err;
84 }
85
86 err = metal_register_generic_device(&vr->shm_device);
87 if (err != 0) {
88 return err;
89 }
90
91 err = metal_device_open("generic", SHM_DEVICE_NAME, &device);
92 if (err != 0) {
93 return err;
94 }
95
96 vr->shm_io = metal_device_io_region(device, 0);
97 if (vr->shm_io == NULL) {
98 return err;
99 }
100
101 return 0;
102 }
103
libmetal_teardown(struct ipc_static_vrings * vr)104 static int libmetal_teardown(struct ipc_static_vrings *vr)
105 {
106 vr->shm_io = 0;
107
108 metal_device_close(&vr->shm_device);
109
110 metal_finish();
111
112 return 0;
113 }
114
vq_setup(struct ipc_static_vrings * vr,unsigned int role)115 static int vq_setup(struct ipc_static_vrings *vr, unsigned int role)
116 {
117 vr->vq[RPMSG_VQ_0] = virtqueue_allocate(vr->vring_size);
118 if (vr->vq[RPMSG_VQ_0] == NULL) {
119 return -ENOMEM;
120 }
121
122 vr->vq[RPMSG_VQ_1] = virtqueue_allocate(vr->vring_size);
123 if (vr->vq[RPMSG_VQ_1] == NULL) {
124 return -ENOMEM;
125 }
126
127 vr->rvrings[RPMSG_VQ_0].io = vr->shm_io;
128 vr->rvrings[RPMSG_VQ_0].info.vaddr = (void *) vr->tx_addr;
129 vr->rvrings[RPMSG_VQ_0].info.num_descs = vr->vring_size;
130 vr->rvrings[RPMSG_VQ_0].info.align = MEM_ALIGNMENT;
131 vr->rvrings[RPMSG_VQ_0].vq = vr->vq[RPMSG_VQ_0];
132
133 vr->rvrings[RPMSG_VQ_1].io = vr->shm_io;
134 vr->rvrings[RPMSG_VQ_1].info.vaddr = (void *) vr->rx_addr;
135 vr->rvrings[RPMSG_VQ_1].info.num_descs = vr->vring_size;
136 vr->rvrings[RPMSG_VQ_1].info.align = MEM_ALIGNMENT;
137 vr->rvrings[RPMSG_VQ_1].vq = vr->vq[RPMSG_VQ_1];
138
139 vr->vdev.role = role;
140
141 vr->vdev.vrings_num = VRING_COUNT;
142 vr->vdev.func = &dispatch;
143 vr->vdev.vrings_info = &vr->rvrings[0];
144
145 return 0;
146 }
147
vq_teardown(struct ipc_static_vrings * vr,unsigned int role)148 static int vq_teardown(struct ipc_static_vrings *vr, unsigned int role)
149 {
150 memset(&vr->vdev, 0, sizeof(struct virtio_device));
151
152 memset(&(vr->rvrings[RPMSG_VQ_1]), 0, sizeof(struct virtio_vring_info));
153 memset(&(vr->rvrings[RPMSG_VQ_0]), 0, sizeof(struct virtio_vring_info));
154
155 virtqueue_free(vr->vq[RPMSG_VQ_1]);
156 virtqueue_free(vr->vq[RPMSG_VQ_0]);
157
158 return 0;
159 }
160
ipc_static_vrings_init(struct ipc_static_vrings * vr,unsigned int role)161 int ipc_static_vrings_init(struct ipc_static_vrings *vr, unsigned int role)
162 {
163 int err = 0;
164
165 if (!vr) {
166 return -EINVAL;
167 }
168
169 vr->shm_device.name = SHM_DEVICE_NAME;
170 vr->shm_device.num_regions = 1;
171 vr->shm_physmap[0] = vr->shm_addr;
172
173 metal_io_init(vr->shm_device.regions, (void *) vr->shm_addr,
174 vr->shm_physmap, vr->shm_size, -1, 0, NULL);
175
176 err = libmetal_setup(vr);
177 if (err != 0) {
178 return err;
179 }
180
181 return vq_setup(vr, role);
182 }
183
ipc_static_vrings_deinit(struct ipc_static_vrings * vr,unsigned int role)184 int ipc_static_vrings_deinit(struct ipc_static_vrings *vr, unsigned int role)
185 {
186 int err;
187
188 err = vq_teardown(vr, role);
189 if (err != 0) {
190 return err;
191 }
192
193 err = libmetal_teardown(vr);
194 if (err != 0) {
195 return err;
196 }
197
198 metal_io_finish(vr->shm_device.regions);
199
200 return 0;
201 }
202
