virtio_ring.c 8.9 KB

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