virtio_ring.c 9.0 KB

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