virtio_ring.c 8.9 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359
  1. // SPDX-License-Identifier: GPL-2.0+
  2. /*
  3. * Copyright (C) 2018, Tuomas Tynkkynen <tuomas.tynkkynen@iki.fi>
  4. * Copyright (C) 2018, Bin Meng <bmeng.cn@gmail.com>
  5. *
  6. * virtio ring implementation
  7. */
  8. #include <common.h>
  9. #include <dm.h>
  10. #include <malloc.h>
  11. #include <virtio_types.h>
  12. #include <virtio.h>
  13. #include <virtio_ring.h>
  14. #include <linux/compat.h>
  15. int virtqueue_add(struct virtqueue *vq, struct virtio_sg *sgs[],
  16. unsigned int out_sgs, unsigned int in_sgs)
  17. {
  18. struct vring_desc *desc;
  19. unsigned int total_sg = out_sgs + in_sgs;
  20. unsigned int i, n, avail, descs_used, uninitialized_var(prev);
  21. int head;
  22. WARN_ON(total_sg == 0);
  23. head = vq->free_head;
  24. desc = vq->vring.desc;
  25. i = head;
  26. descs_used = total_sg;
  27. if (vq->num_free < descs_used) {
  28. debug("Can't add buf len %i - avail = %i\n",
  29. descs_used, vq->num_free);
  30. /*
  31. * FIXME: for historical reasons, we force a notify here if
  32. * there are outgoing parts to the buffer. Presumably the
  33. * host should service the ring ASAP.
  34. */
  35. if (out_sgs)
  36. virtio_notify(vq->vdev, vq);
  37. return -ENOSPC;
  38. }
  39. for (n = 0; n < out_sgs; n++) {
  40. struct virtio_sg *sg = sgs[n];
  41. desc[i].flags = cpu_to_virtio16(vq->vdev, VRING_DESC_F_NEXT);
  42. desc[i].addr = cpu_to_virtio64(vq->vdev, (u64)(size_t)sg->addr);
  43. desc[i].len = cpu_to_virtio32(vq->vdev, sg->length);
  44. prev = i;
  45. i = virtio16_to_cpu(vq->vdev, desc[i].next);
  46. }
  47. for (; n < (out_sgs + in_sgs); n++) {
  48. struct virtio_sg *sg = sgs[n];
  49. desc[i].flags = cpu_to_virtio16(vq->vdev, VRING_DESC_F_NEXT |
  50. VRING_DESC_F_WRITE);
  51. desc[i].addr = cpu_to_virtio64(vq->vdev,
  52. (u64)(uintptr_t)sg->addr);
  53. desc[i].len = cpu_to_virtio32(vq->vdev, sg->length);
  54. prev = i;
  55. i = virtio16_to_cpu(vq->vdev, desc[i].next);
  56. }
  57. /* Last one doesn't continue */
  58. desc[prev].flags &= cpu_to_virtio16(vq->vdev, ~VRING_DESC_F_NEXT);
  59. /* We're using some buffers from the free list. */
  60. vq->num_free -= descs_used;
  61. /* Update free pointer */
  62. vq->free_head = i;
  63. /*
  64. * Put entry in available array (but don't update avail->idx
  65. * until they do sync).
  66. */
  67. avail = vq->avail_idx_shadow & (vq->vring.num - 1);
  68. vq->vring.avail->ring[avail] = cpu_to_virtio16(vq->vdev, head);
  69. /*
  70. * Descriptors and available array need to be set before we expose the
  71. * new available array entries.
  72. */
  73. virtio_wmb();
  74. vq->avail_idx_shadow++;
  75. vq->vring.avail->idx = cpu_to_virtio16(vq->vdev, vq->avail_idx_shadow);
  76. vq->num_added++;
  77. /*
  78. * This is very unlikely, but theoretically possible.
  79. * Kick just in case.
  80. */
  81. if (unlikely(vq->num_added == (1 << 16) - 1))
  82. virtqueue_kick(vq);
  83. return 0;
  84. }
  85. static bool virtqueue_kick_prepare(struct virtqueue *vq)
  86. {
  87. u16 new, old;
  88. bool needs_kick;
  89. /*
  90. * We need to expose available array entries before checking
  91. * avail event.
  92. */
  93. virtio_mb();
  94. old = vq->avail_idx_shadow - vq->num_added;
  95. new = vq->avail_idx_shadow;
  96. vq->num_added = 0;
  97. if (vq->event) {
  98. needs_kick = vring_need_event(virtio16_to_cpu(vq->vdev,
  99. vring_avail_event(&vq->vring)), new, old);
  100. } else {
  101. needs_kick = !(vq->vring.used->flags & cpu_to_virtio16(vq->vdev,
  102. VRING_USED_F_NO_NOTIFY));
  103. }
  104. return needs_kick;
  105. }
  106. void virtqueue_kick(struct virtqueue *vq)
  107. {
  108. if (virtqueue_kick_prepare(vq))
  109. virtio_notify(vq->vdev, vq);
  110. }
  111. static void detach_buf(struct virtqueue *vq, unsigned int head)
  112. {
  113. unsigned int i;
  114. __virtio16 nextflag = cpu_to_virtio16(vq->vdev, VRING_DESC_F_NEXT);
  115. /* Put back on free list: unmap first-level descriptors and find end */
  116. i = head;
  117. while (vq->vring.desc[i].flags & nextflag) {
  118. i = virtio16_to_cpu(vq->vdev, vq->vring.desc[i].next);
  119. vq->num_free++;
  120. }
  121. vq->vring.desc[i].next = cpu_to_virtio16(vq->vdev, vq->free_head);
  122. vq->free_head = head;
  123. /* Plus final descriptor */
  124. vq->num_free++;
  125. }
  126. static inline bool more_used(const struct virtqueue *vq)
  127. {
  128. return vq->last_used_idx != virtio16_to_cpu(vq->vdev,
  129. vq->vring.used->idx);
  130. }
  131. void *virtqueue_get_buf(struct virtqueue *vq, unsigned int *len)
  132. {
  133. unsigned int i;
  134. u16 last_used;
  135. if (!more_used(vq)) {
  136. debug("(%s.%d): No more buffers in queue\n",
  137. vq->vdev->name, vq->index);
  138. return NULL;
  139. }
  140. /* Only get used array entries after they have been exposed by host */
  141. virtio_rmb();
  142. last_used = (vq->last_used_idx & (vq->vring.num - 1));
  143. i = virtio32_to_cpu(vq->vdev, vq->vring.used->ring[last_used].id);
  144. if (len) {
  145. *len = virtio32_to_cpu(vq->vdev,
  146. vq->vring.used->ring[last_used].len);
  147. debug("(%s.%d): last used idx %u with len %u\n",
  148. vq->vdev->name, vq->index, i, *len);
  149. }
  150. if (unlikely(i >= vq->vring.num)) {
  151. printf("(%s.%d): id %u out of range\n",
  152. vq->vdev->name, vq->index, i);
  153. return NULL;
  154. }
  155. detach_buf(vq, i);
  156. vq->last_used_idx++;
  157. /*
  158. * If we expect an interrupt for the next entry, tell host
  159. * by writing event index and flush out the write before
  160. * the read in the next get_buf call.
  161. */
  162. if (!(vq->avail_flags_shadow & VRING_AVAIL_F_NO_INTERRUPT))
  163. virtio_store_mb(&vring_used_event(&vq->vring),
  164. cpu_to_virtio16(vq->vdev, vq->last_used_idx));
  165. return (void *)(uintptr_t)virtio64_to_cpu(vq->vdev,
  166. vq->vring.desc[i].addr);
  167. }
  168. static struct virtqueue *__vring_new_virtqueue(unsigned int index,
  169. struct vring vring,
  170. struct udevice *udev)
  171. {
  172. unsigned int i;
  173. struct virtqueue *vq;
  174. struct virtio_dev_priv *uc_priv = dev_get_uclass_priv(udev);
  175. struct udevice *vdev = uc_priv->vdev;
  176. vq = malloc(sizeof(*vq));
  177. if (!vq)
  178. return NULL;
  179. vq->vdev = vdev;
  180. vq->index = index;
  181. vq->num_free = vring.num;
  182. vq->vring = vring;
  183. vq->last_used_idx = 0;
  184. vq->avail_flags_shadow = 0;
  185. vq->avail_idx_shadow = 0;
  186. vq->num_added = 0;
  187. list_add_tail(&vq->list, &uc_priv->vqs);
  188. vq->event = virtio_has_feature(vdev, VIRTIO_RING_F_EVENT_IDX);
  189. /* Tell other side not to bother us */
  190. vq->avail_flags_shadow |= VRING_AVAIL_F_NO_INTERRUPT;
  191. if (!vq->event)
  192. vq->vring.avail->flags = cpu_to_virtio16(vdev,
  193. vq->avail_flags_shadow);
  194. /* Put everything in free lists */
  195. vq->free_head = 0;
  196. for (i = 0; i < vring.num - 1; i++)
  197. vq->vring.desc[i].next = cpu_to_virtio16(vdev, i + 1);
  198. return vq;
  199. }
  200. struct virtqueue *vring_create_virtqueue(unsigned int index, unsigned int num,
  201. unsigned int vring_align,
  202. struct udevice *udev)
  203. {
  204. struct virtqueue *vq;
  205. void *queue = NULL;
  206. struct vring vring;
  207. /* We assume num is a power of 2 */
  208. if (num & (num - 1)) {
  209. printf("Bad virtqueue length %u\n", num);
  210. return NULL;
  211. }
  212. /* TODO: allocate each queue chunk individually */
  213. for (; num && vring_size(num, vring_align) > PAGE_SIZE; num /= 2) {
  214. queue = memalign(PAGE_SIZE, vring_size(num, vring_align));
  215. if (queue)
  216. break;
  217. }
  218. if (!num)
  219. return NULL;
  220. if (!queue) {
  221. /* Try to get a single page. You are my only hope! */
  222. queue = memalign(PAGE_SIZE, vring_size(num, vring_align));
  223. }
  224. if (!queue)
  225. return NULL;
  226. memset(queue, 0, vring_size(num, vring_align));
  227. vring_init(&vring, num, queue, vring_align);
  228. vq = __vring_new_virtqueue(index, vring, udev);
  229. if (!vq) {
  230. free(queue);
  231. return NULL;
  232. }
  233. debug("(%s): created vring @ %p for vq @ %p with num %u\n", udev->name,
  234. queue, vq, num);
  235. return vq;
  236. }
  237. void vring_del_virtqueue(struct virtqueue *vq)
  238. {
  239. free(vq->vring.desc);
  240. list_del(&vq->list);
  241. free(vq);
  242. }
  243. unsigned int virtqueue_get_vring_size(struct virtqueue *vq)
  244. {
  245. return vq->vring.num;
  246. }
  247. ulong virtqueue_get_desc_addr(struct virtqueue *vq)
  248. {
  249. return (ulong)vq->vring.desc;
  250. }
  251. ulong virtqueue_get_avail_addr(struct virtqueue *vq)
  252. {
  253. return (ulong)vq->vring.desc +
  254. ((char *)vq->vring.avail - (char *)vq->vring.desc);
  255. }
  256. ulong virtqueue_get_used_addr(struct virtqueue *vq)
  257. {
  258. return (ulong)vq->vring.desc +
  259. ((char *)vq->vring.used - (char *)vq->vring.desc);
  260. }
  261. bool virtqueue_poll(struct virtqueue *vq, u16 last_used_idx)
  262. {
  263. virtio_mb();
  264. return last_used_idx != virtio16_to_cpu(vq->vdev, vq->vring.used->idx);
  265. }
  266. void virtqueue_dump(struct virtqueue *vq)
  267. {
  268. unsigned int i;
  269. printf("virtqueue %p for dev %s:\n", vq, vq->vdev->name);
  270. printf("\tindex %u, phys addr %p num %u\n",
  271. vq->index, vq->vring.desc, vq->vring.num);
  272. printf("\tfree_head %u, num_added %u, num_free %u\n",
  273. vq->free_head, vq->num_added, vq->num_free);
  274. printf("\tlast_used_idx %u, avail_flags_shadow %u, avail_idx_shadow %u\n",
  275. vq->last_used_idx, vq->avail_flags_shadow, vq->avail_idx_shadow);
  276. printf("Descriptor dump:\n");
  277. for (i = 0; i < vq->vring.num; i++) {
  278. printf("\tdesc[%u] = { 0x%llx, len %u, flags %u, next %u }\n",
  279. i, vq->vring.desc[i].addr, vq->vring.desc[i].len,
  280. vq->vring.desc[i].flags, vq->vring.desc[i].next);
  281. }
  282. printf("Avail ring dump:\n");
  283. printf("\tflags %u, idx %u\n",
  284. vq->vring.avail->flags, vq->vring.avail->idx);
  285. for (i = 0; i < vq->vring.num; i++) {
  286. printf("\tavail[%u] = %u\n",
  287. i, vq->vring.avail->ring[i]);
  288. }
  289. printf("Used ring dump:\n");
  290. printf("\tflags %u, idx %u\n",
  291. vq->vring.used->flags, vq->vring.used->idx);
  292. for (i = 0; i < vq->vring.num; i++) {
  293. printf("\tused[%u] = { %u, %u }\n", i,
  294. vq->vring.used->ring[i].id, vq->vring.used->ring[i].len);
  295. }
  296. }