static inline void
_vq_record(uint32_t offs, int i, volatile struct vring_desc *vd,
struct iovec *iov, int n_iov, uint16_t *flags) {
if (i >= n_iov)
return;
iov[i].iov_base = paddr_map(offs, be64toh(vd->addr),
be32toh(vd->len));
iov[i].iov_len = be32toh(vd->len);
if (flags != NULL)
flags[i] = be16toh(vd->flags);
}
int
vq_getchain(uint32_t offs, struct vqueue_info *vq,
struct iovec *iov, int n_iov, uint16_t *flags)
{
volatile struct vring_desc *vdir, *vindir, *vp;
int idx, ndesc, n_indir;
int head, next;
int i;
idx = vq->vq_last_avail;
ndesc = (be16toh(vq->vq_avail->idx) - idx);
if (ndesc == 0)
return (0);
head = be16toh(vq->vq_avail->ring[idx & (vq->vq_qsize - 1)]);
next = head;
for (i = 0; i < VQ_MAX_DESCRIPTORS; next = be16toh(vdir->next)) {
vdir = &vq->vq_desc[next];
if ((be16toh(vdir->flags) & VRING_DESC_F_INDIRECT) == 0) {
_vq_record(offs, i, vdir, iov, n_iov, flags);
i++;
} else {
n_indir = be32toh(vdir->len) / 16;
vindir = paddr_map(offs, be64toh(vdir->addr),
be32toh(vdir->len));
next = 0;
for (;;) {
vp = &vindir[next];
_vq_record(offs, i, vp, iov, n_iov, flags);
i+=1;
if ((be16toh(vp->flags) & \
VRING_DESC_F_NEXT) == 0)
break;
next = be16toh(vp->next);
}
paddr_unmap((void *)vindir, be32toh(vdir->len));
}
if ((be16toh(vdir->flags) & VRING_DESC_F_NEXT) == 0)
return (i);
}
return (i);
}
static int
vq_init(struct vtbe_softc *sc)
{
struct vqueue_info *vq;
uint8_t *base;
int size;
int reg;
int pfn;
vq = &sc->vs_queues[sc->vs_curq];
vq->vq_qsize = DESC_COUNT;
reg = READ4(sc, VIRTIO_MMIO_QUEUE_PFN);
pfn = be32toh(reg);
vq->vq_pfn = pfn;
size = vring_size(vq->vq_qsize, VRING_ALIGN);
base = paddr_map(sc->beri_mem_offset,
(pfn << PAGE_SHIFT), size);
/* First pages are descriptors */
vq->vq_desc = (struct vring_desc *)base;
base += vq->vq_qsize * sizeof(struct vring_desc);
/* Then avail ring */
vq->vq_avail = (struct vring_avail *)base;
base += (2 + vq->vq_qsize + 1) * sizeof(uint16_t);
/* Then it's rounded up to the next page */
base = (uint8_t *)roundup2((uintptr_t)base, VRING_ALIGN);
/* And the last pages are the used ring */
vq->vq_used = (struct vring_used *)base;
/* Mark queue as allocated, and start at 0 when we use it. */
vq->vq_flags = VQ_ALLOC;
vq->vq_last_avail = 0;
return (0);
}
