|
| 1 | +/* |
| 2 | + * Copyright (c) 2025 TOKITA Hiroshi |
| 3 | + * |
| 4 | + * SPDX-License-Identifier: Apache-2.0 |
| 5 | + */ |
| 6 | + |
| 7 | +#include <zephyr/drivers/vhost.h> |
| 8 | +#include <zephyr/drivers/vhost/vringh.h> |
| 9 | +#include <zephyr/logging/log.h> |
| 10 | + |
| 11 | +LOG_MODULE_REGISTER(vhost); |
| 12 | + |
| 13 | +struct vhost_iovec riovec[16]; |
| 14 | +struct vhost_iovec wiovec[16]; |
| 15 | + |
| 16 | +struct vringh_iov riov = { |
| 17 | + .iov = riovec, |
| 18 | + .max_num = 16, |
| 19 | +}; |
| 20 | + |
| 21 | +struct vringh_iov wiov = { |
| 22 | + .iov = wiovec, |
| 23 | + .max_num = 16, |
| 24 | +}; |
| 25 | + |
| 26 | +struct vringh vrh_inst; |
| 27 | + |
| 28 | +void vringh_handler(struct vringh *vrh) |
| 29 | +{ |
| 30 | + LOG_DBG("%s: queue_id=%lu", __func__, vrh->queue_id); |
| 31 | + uint16_t head; |
| 32 | + |
| 33 | + while (true) { |
| 34 | + int ret = vringh_getdesc(vrh, &riov, &wiov, &head); |
| 35 | + |
| 36 | + if (ret < 0) { |
| 37 | + LOG_ERR("vringh_getdesc failed: %d", ret); |
| 38 | + return; |
| 39 | + } |
| 40 | + |
| 41 | + if (ret == 0) { |
| 42 | + return; |
| 43 | + } |
| 44 | + |
| 45 | + /* Process writable iovecs */ |
| 46 | + for (uint32_t s = 0; s < wiov.used; s++) { |
| 47 | + uint8_t *dst = wiov.iov[s].iov_base; |
| 48 | + uint32_t len = wiov.iov[s].iov_len; |
| 49 | + |
| 50 | + LOG_DBG("%s: addr=%p len=%u", __func__, dst, len); |
| 51 | + |
| 52 | + for (uint32_t i = 0; i < len; i++) { |
| 53 | + sys_write8(i, (mem_addr_t)&dst[i]); |
| 54 | + } |
| 55 | + } |
| 56 | + |
| 57 | + barrier_dmem_fence_full(); |
| 58 | + |
| 59 | + uint32_t total_len = 0; |
| 60 | + for (uint32_t i = 0; i < wiov.used; i++) { |
| 61 | + total_len += wiov.iov[i].iov_len; |
| 62 | + } |
| 63 | + |
| 64 | + vringh_complete(vrh, head, total_len); |
| 65 | + |
| 66 | + if (vringh_need_notify(vrh) > 0) { |
| 67 | + vringh_notify(vrh); |
| 68 | + } |
| 69 | + |
| 70 | + /* Reset iovecs for next iteration */ |
| 71 | + vringh_iov_reset(&riov); |
| 72 | + vringh_iov_reset(&wiov); |
| 73 | + } |
| 74 | +} |
| 75 | + |
| 76 | +void queue_ready_handler(const struct device *dev, uint16_t qid, void *data) |
| 77 | +{ |
| 78 | + LOG_DBG("%s(dev=%p, qid=%u, data=%p)", __func__, dev, qid, data); |
| 79 | + |
| 80 | + /* Initialize iovecs before descriptor processing */ |
| 81 | + vringh_iov_init(&riov, riov.iov, riov.max_num); |
| 82 | + vringh_iov_init(&wiov, wiov.iov, wiov.max_num); |
| 83 | + |
| 84 | + int err = vringh_init_device(&vrh_inst, dev, qid, vringh_handler); |
| 85 | + if (err) { |
| 86 | + LOG_ERR("vringh_init_device failed: %d", err); |
| 87 | + return; |
| 88 | + } |
| 89 | +} |
| 90 | + |
| 91 | +int main(void) |
| 92 | +{ |
| 93 | + const struct device *device = DEVICE_DT_GET(DT_NODELABEL(vhost)); |
| 94 | + |
| 95 | + if (!device_is_ready(device)) { |
| 96 | + LOG_ERR("VHost device not ready"); |
| 97 | + return -ENODEV; |
| 98 | + } |
| 99 | + |
| 100 | + vhost_set_ready_callback(device, queue_ready_handler, (void *)device); |
| 101 | + |
| 102 | + k_sleep(K_FOREVER); |
| 103 | + return 0; |
| 104 | +} |
0 commit comments