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 RPMSG_VQ_0 (0) /* TX virtqueue queue index */
11 #define RPMSG_VQ_1 (1) /* RX virtqueue queue index */
12
ipc_virtio_notify(struct virtqueue * vq)13 static void ipc_virtio_notify(struct virtqueue *vq)
14 {
15 struct ipc_static_vrings *vr;
16
17 vr = CONTAINER_OF(vq->vq_dev, struct ipc_static_vrings, vdev);
18
19 if (vr->notify_cb) {
20 vr->notify_cb(vq, vr->priv);
21 }
22 }
23
ipc_virtio_set_features(struct virtio_device * vdev,uint32_t features)24 static void ipc_virtio_set_features(struct virtio_device *vdev, uint32_t features)
25 {
26 /* No need for implementation */
27 }
28
ipc_virtio_set_status(struct virtio_device * p_vdev,unsigned char status)29 static void ipc_virtio_set_status(struct virtio_device *p_vdev, unsigned char status)
30 {
31 struct ipc_static_vrings *vr;
32
33 if (p_vdev->role != VIRTIO_DEV_DRIVER) {
34 return;
35 }
36
37 vr = CONTAINER_OF(p_vdev, struct ipc_static_vrings, vdev);
38
39 sys_write8(status, vr->status_reg_addr);
40 sys_cache_data_flush_range((void *) vr->status_reg_addr, sizeof(status));
41 }
42
ipc_virtio_get_features(struct virtio_device * vdev)43 static uint32_t ipc_virtio_get_features(struct virtio_device *vdev)
44 {
45 return BIT(VIRTIO_RPMSG_F_NS);
46 }
47
ipc_virtio_get_status(struct virtio_device * p_vdev)48 static unsigned char ipc_virtio_get_status(struct virtio_device *p_vdev)
49 {
50 struct ipc_static_vrings *vr;
51 uint8_t ret;
52
53 vr = CONTAINER_OF(p_vdev, struct ipc_static_vrings, vdev);
54
55 ret = VIRTIO_CONFIG_STATUS_DRIVER_OK;
56
57 if (p_vdev->role == VIRTIO_DEV_DEVICE) {
58 sys_cache_data_invd_range((void *) vr->status_reg_addr, sizeof(ret));
59 ret = sys_read8(vr->status_reg_addr);
60 }
61
62 return ret;
63 }
64
65 const static struct virtio_dispatch dispatch = {
66 .get_status = ipc_virtio_get_status,
67 .get_features = ipc_virtio_get_features,
68 .set_status = ipc_virtio_set_status,
69 .set_features = ipc_virtio_set_features,
70 .notify = ipc_virtio_notify,
71 };
72
vq_setup(struct ipc_static_vrings * vr,unsigned int role)73 static int vq_setup(struct ipc_static_vrings *vr, unsigned int role)
74 {
75 vr->vq[RPMSG_VQ_0] = virtqueue_allocate(vr->vring_size);
76 if (vr->vq[RPMSG_VQ_0] == NULL) {
77 return -ENOMEM;
78 }
79
80 vr->vq[RPMSG_VQ_1] = virtqueue_allocate(vr->vring_size);
81 if (vr->vq[RPMSG_VQ_1] == NULL) {
82 return -ENOMEM;
83 }
84
85 vr->rvrings[RPMSG_VQ_0].io = &vr->shm_io;
86 vr->rvrings[RPMSG_VQ_0].info.vaddr = (void *) vr->tx_addr;
87 vr->rvrings[RPMSG_VQ_0].info.num_descs = vr->vring_size;
88 vr->rvrings[RPMSG_VQ_0].info.align = MEM_ALIGNMENT;
89 vr->rvrings[RPMSG_VQ_0].vq = vr->vq[RPMSG_VQ_0];
90
91 vr->rvrings[RPMSG_VQ_1].io = &vr->shm_io;
92 vr->rvrings[RPMSG_VQ_1].info.vaddr = (void *) vr->rx_addr;
93 vr->rvrings[RPMSG_VQ_1].info.num_descs = vr->vring_size;
94 vr->rvrings[RPMSG_VQ_1].info.align = MEM_ALIGNMENT;
95 vr->rvrings[RPMSG_VQ_1].vq = vr->vq[RPMSG_VQ_1];
96
97 vr->vdev.role = role;
98
99 vr->vdev.vrings_num = VRING_COUNT;
100 vr->vdev.func = &dispatch;
101 vr->vdev.vrings_info = &vr->rvrings[0];
102
103 return 0;
104 }
105
vq_teardown(struct ipc_static_vrings * vr,unsigned int role)106 static int vq_teardown(struct ipc_static_vrings *vr, unsigned int role)
107 {
108 memset(&vr->vdev, 0, sizeof(struct virtio_device));
109
110 memset(&(vr->rvrings[RPMSG_VQ_1]), 0, sizeof(struct virtio_vring_info));
111 memset(&(vr->rvrings[RPMSG_VQ_0]), 0, sizeof(struct virtio_vring_info));
112
113 virtqueue_free(vr->vq[RPMSG_VQ_1]);
114 virtqueue_free(vr->vq[RPMSG_VQ_0]);
115
116 return 0;
117 }
118
ipc_static_vrings_init(struct ipc_static_vrings * vr,unsigned int role)119 int ipc_static_vrings_init(struct ipc_static_vrings *vr, unsigned int role)
120 {
121 struct metal_init_params metal_params = METAL_INIT_DEFAULTS;
122 int err = 0;
123
124 if (!vr) {
125 return -EINVAL;
126 }
127
128 err = metal_init(&metal_params);
129 if (err != 0) {
130 return err;
131 }
132
133 vr->shm_physmap[0] = vr->shm_addr;
134
135 metal_io_init(&vr->shm_io, (void *)vr->shm_addr,
136 vr->shm_physmap, vr->shm_size, -1, 0, NULL);
137
138 return vq_setup(vr, role);
139 }
140
ipc_static_vrings_deinit(struct ipc_static_vrings * vr,unsigned int role)141 int ipc_static_vrings_deinit(struct ipc_static_vrings *vr, unsigned int role)
142 {
143 int err;
144
145 err = vq_teardown(vr, role);
146 if (err != 0) {
147 return err;
148 }
149
150 metal_io_finish(&vr->shm_io);
151
152 metal_finish();
153
154 return 0;
155 }
156
