#include <u.h>
#include <libc.h>
#include <thread.h>
#include "dat.h"
#include "fns.h"

#include <ip.h>         /* parseether() */
#include <libsec.h>     /* genrandom() */

typedef struct VIODev VIODev;
typedef struct VIOQueue VIOQueue;
typedef struct VIOBuf VIOBuf;
typedef struct VIONetDev VIONetDev;
typedef struct VIOBlkDev VIOBlkDev;

enum {
       BUFCHAIN = 1,
       BUFWR = 2,

       USEDNOIRQ = 1,

       DRIVEROK = 4, /* devstat */
};

struct VIOBuf {
       u32int flags;
       VIOQueue *qu;
       void *p;
       u64int addr;
       u32int len;
       u32int idx;
       VIOBuf *next, *head;
       u32int rptr, wptr;
};

struct VIOQueue {
       QLock;
       Rendez;
       VIODev *d;
       u8int (*desc)[16], *avail, *used;
       u16int size;
       u32int addr;
       u16int availidx, usedidx;
       void (*notify)(VIOQueue*);
       int livebuf;
       Rendez livebufrend;
};

struct VIONetDev {
       int readfd, writefd;
       u8int mac[6];
       enum {
               VNETPROMISC = 1,
               VNETALLMULTI = 2,
               VNETALLUNI = 4,
               VNETNOMULTI = 8,
               VNETNOUNI = 16,
               VNETNOBCAST = 32,

               VNETHEADER = 1<<31,
       } flags;
       u64int macbloom, multibloom;
};

struct VIOBlkDev {
       int fd;
       uvlong size;
};

struct VIODev {
       PCIDev *pci;
       u32int devfeat, guestfeat;
       u16int qsel;
       u8int devstat, isrstat;
       VIOQueue *qu;
       int nqu, allocqu;
       u32int (*io)(int, u16int, u32int, int, VIODev *);
       void (*reset)(VIODev *);
       union {
               VIONetDev net;
               VIOBlkDev blk;
       };
};

static void
vioirq_(void *arg)
{
       VIODev *d;
       int val;

       d = ((void**)arg)[0];
       val = (uintptr)((void**)arg)[1];
       if(val != 0)
               d->isrstat |= val;
       else
               d->isrstat = 0;
       pciirq(d->pci, d->isrstat);
       free(arg);
}

static void
vioirq(VIODev *d, int val)
{
       void **v;

       assert(d != nil);
       v = emalloc(sizeof(void*)*2);
       v[0] = d;
       v[1] = (void *) val;
       sendnotif(vioirq_, v);
}

static void *
checkdesc(VIOQueue *q, int i)
{
       if(i >= q->size){
               vmerror("virtio device %#x: invalid next pointer %d in queue (size %d), ignoring descriptor", q->d->pci->bdf, i, q->size);
               return nil;
       }
       return q->desc[i];
}

VIOBuf *
viogetbuf(VIOQueue *q, int wait)
{
       u16int gidx;
       VIOBuf *b, *rb, **bp;
       void *dp;

       qlock(q);
waitloop:
       while((q->d->devstat & DRIVEROK) == 0 || q->desc == nil || (gidx = GET16(q->avail, 2), gidx == q->availidx)){
               if(!wait){
                       qunlock(q);
                       return nil;
               }
               rsleep(q);
       }
       dp = checkdesc(q, GET16(q->avail, 4 + 2 * (q->availidx % q->size)));
       rb = nil;
       bp = &rb;
       for(;;){
               b = emalloc(sizeof(VIOBuf));
               b->qu = q;
               b->idx = (u8int(*)[16])dp - q->desc;
               b->addr = GET64(dp, 0);
               b->len = GET32(dp, 8);
               b->flags = GET16(dp, 12);
               b->p = gptr(b->addr, b->len);
               if(b->p == nil){
                       vmerror("virtio device %#x: invalid buffer pointer %#p in queue, ignoring descriptor", q->d->pci->bdf, (void*)b->addr);
                       free(b);
                       break;
               }
               *bp = b;
               b->head = rb;
               bp = &b->next;
               if((b->flags & BUFCHAIN) == 0) break;
               dp = checkdesc(q, GET16(dp, 14));
               if(dp == nil) break;
       }
       q->availidx++;
       if(rb == nil) goto waitloop;
       q->livebuf++;
       qunlock(q);
       return rb;
}

void
vioputbuf(VIOBuf *b)
{
       VIOBuf *bn;
       VIOQueue *q;
       u8int *p;

       if(b == nil) return;
       q = b->qu;
       qlock(q);
       if((q->d->devstat & DRIVEROK) == 0){
               qunlock(q);
               goto end;
       }
       if(q->used == nil)
               vmerror("virtio device %#x: address was set to an invalid value while holding buffer", q->d->pci->bdf);
       else{
               p = q->used + 4 + 8 * (q->usedidx % q->size);
               PUT32(p, 4, b->wptr);
               PUT32(p, 0, b->idx);
               PUT16(q->used, 2, ++q->usedidx);
       }
       if(--q->livebuf <= 0)
               rwakeup(&q->livebufrend);
       qunlock(q);
       if(q->avail != nil && (GET16(q->avail, 0) & USEDNOIRQ) == 0)
               vioirq(q->d, 1);
end:
       while(b != nil){
               bn = b->next;
               free(b);
               b = bn;
       }
}

ulong
vioqread(VIOBuf *b, void *v, ulong n)
{
       VIOBuf *c;
       u32int p;
       int rc;
       ulong m;

       p = b->rptr;
       c = b;
       rc = 0;
       for(;;){
               if(rc >= n) return rc;
               for(;;){
                       if(c == nil) return rc;
                       if((c->flags & BUFWR) == 0){
                               if(p < c->len) break;
                               p -= c->len;
                       }
                       c = c->next;
               }
               m = c->len - p;
               if(m > n - rc) m = n - rc;
               memmove(v, (u8int*)c->p + p, m);
               p += m, rc += m;
               v = (u8int*)v + m;
               b->rptr += m;
       }
}

ulong
vioqwrite(VIOBuf *b, void *v, ulong n)
{
       VIOBuf *c;
       u32int p;
       int rc;
       ulong m;

       p = b->wptr;
       c = b;
       rc = 0;
       for(;;){
               if(rc >= n) return rc;
               for(;;){
                       if(c == nil) return rc;
                       if((c->flags & BUFWR) != 0){
                               if(p < c->len) break;
                               p -= c->len;
                       }
                       c = c->next;
               }
               m = c->len - p;
               if(m > n - rc) m = n - rc;
               memmove((u8int*)c->p + p, v, m);
               p += m, rc += m;
               v = (u8int*)v + m;
               b->wptr += m;
       }
}

ulong
vioqrem(VIOBuf *b, int wr)
{
       VIOBuf *c;
       u32int p;
       ulong rc;

       p = wr ? b->wptr : b->rptr;
       for(c = b;; c = c->next){
               if(c == nil) return 0;
               if(((c->flags & BUFWR) != 0) == wr){
                       if(p < c->len) break;
                       p -= c->len;
               }
       }
       rc = c->len - p;
       for(c = c->next; c != nil; c = c->next)
               if(((c->flags & BUFWR) != 0) == wr)
                       rc += c->len;
       return rc;
}

static void
vioqaddrset(VIOQueue *q, u64int addr)
{
       void *p;
       int sz1, sz;

       addr <<= 12;
       sz1 = -(-(18 * q->size + 4) & -4096);
       sz = sz1 + (-(-(8 * q->size + 6) & -4096));
       p = gptr(addr, sz);
       if(p == nil)
               vmerror("virtio device %#x: attempt to set queue to invalid address %#p", q->d->pci->bdf, (void *) addr);
       qlock(q);
       q->addr = addr;
       if(p == nil){
               q->desc = nil;
               q->avail = nil;
               q->used = nil;
       }else{
               q->desc = p;
               q->avail = (u8int*)p + 16 * q->size;
               q->used = (u8int*)p + sz1;
               rwakeupall(q);
       }
       qunlock(q);
}

static void
viodevstatset(VIODev *v, u32int val)
{
       int i;

       v->devstat = val;
       if(val == 0){
               if(v->reset != nil)
                       v->reset(v);
               v->guestfeat = 0;
               vioirq(v, 0);
               for(i = 0; i < v->nqu; i++){
                       qlock(&v->qu[i]);
                       while(v->qu[i].livebuf > 0)
                               rsleep(&v->qu[i].livebufrend);
                       qunlock(&v->qu[i]);
               }
       }else{
               for(i = 0; i < v->nqu; i++)
                       v->qu[i].notify(&v->qu[i]);
       }
}

u32int
vioio(int isin, u16int port, u32int val, int sz, void *vp)
{
       VIODev *v;
       int rc;
       static char whinebuf[32];

       v = vp;
       switch(isin << 16 | port){
       case 0x4: v->guestfeat = val; return 0;
       case 0x8: if(v->qsel < v->nqu) vioqaddrset(&v->qu[v->qsel], val); return 0;
       case 0xe: v->qsel = val; return 0;
       case 0x10: if(val < v->nqu) v->qu[val].notify(&v->qu[val]); return 0;
       case 0x12: viodevstatset(v, val); return 0;
       case 0x10000: return v->devfeat;
       case 0x10004: return v->guestfeat;
       case 0x10008: return v->qsel >= v->nqu ? 0 : v->qu[v->qsel].addr;
       case 0x1000c: return v->qsel >= v->nqu ? 0 : v->qu[v->qsel].size;
       case 0x1000e: return v->qsel;
       case 0x10010: return 0;
       case 0x10012: return v->devstat;
       case 0x10013: rc = v->isrstat; vioirq(v, 0); return rc;
       }
       if(port >= 20 && v->io != nil)
               return v->io(isin, port - 20, val, sz, v);
       snprint(whinebuf, sizeof(whinebuf), "virtio device %6x", v->pci->bdf);
       return iowhine(isin, port, val, sz, whinebuf);
}

VIODev *
mkviodev(u16int devid, u32int pciclass, u32int subid, int queues)
{
       VIODev *d;

       d = emalloc(sizeof(VIODev));
       d->pci = mkpcidev(allocbdf(), devid << 16 | 0x1AF4, pciclass << 8, 1);
       d->pci->subid = subid << 16;
       mkpcibar(d->pci, BARIO, 0, 256, vioio, d);
       d->qu = emalloc(queues * sizeof(VIOQueue));
       d->allocqu = queues;
       return d;
}

static void
viowakeup(VIOQueue *q)
{
       qlock(q);
       rwakeupall(q);
       qunlock(q);
}

VIOQueue *
mkvioqueue(VIODev *d, int sz, void (*fn)(VIOQueue*))
{
       VIOQueue *q;

       assert(sz > 0 && sz <= 32768 && (sz & sz - 1) == 0 && fn != nil && d->nqu < d->allocqu);
       q = d->qu + d->nqu++;
       q->Rendez.l = q;
       q->livebufrend.l = q;
       q->size = sz;
       q->d = d;
       q->notify = fn;
       return q;
}

int
bloomhash(u8int *mac)
{
       int x;

       x = mac[0];
       x ^= mac[0] >> 6 ^ mac[1] << 2;
       x ^= mac[1] >> 4 ^ mac[2] << 4;
       x ^= mac[2] >> 2;
       x ^= mac[3];
       x ^= mac[3] >> 6 ^ mac[4] << 2;
       x ^= mac[4] >> 4 ^ mac[5] << 4;
       x ^= mac[5] >> 2;
       return x & 63;
}

int
viomacok(VIODev *d, u8int *mac)
{
       static u8int bcast[6] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff};

       if((d->net.flags & VNETPROMISC) != 0) return 1;
       if((mac[0] & 1) == 0){
               if((d->net.flags & (VNETNOUNI|VNETALLUNI)) != 0)
                       return (d->net.flags & VNETNOUNI) == 0;
               if(memcmp(mac, d->net.mac, 6) == 0) return 1;
               if(d->net.macbloom == 0) return 0;
               return d->net.macbloom >> bloomhash(mac) & 1;
       }else if(memcmp(mac, bcast, 6) == 0)
               return (d->net.flags & VNETNOBCAST) == 0;
       else{
               if((d->net.flags & (VNETNOMULTI|VNETALLMULTI)) != 0)
                       return (d->net.flags & VNETNOMULTI) == 0;
               if(d->net.multibloom == 0) return 0;
               return d->net.multibloom >> bloomhash(mac) & 1;
       }
}

void
vionetrproc(void *vp)
{
       VIODev *v;
       VIOQueue *q;
       VIOBuf *vb;
       uchar rxhead[10];
       uchar rxbuf[1600];
       int rc;

       threadsetname("vionetrproc");
       v = vp;
       q = &v->qu[0];
       memset(rxhead, 0, sizeof(rxhead));
       for(;;){
               rc = read(v->net.readfd, rxbuf, sizeof(rxbuf));
               if(rc == 0){
                       vmerror("read(vionetrproc): eof");
                       threadexits("read: eof");
               }
               if(rc < 0){
                       vmerror("read(vionetrproc): %r");
                       threadexits("read: %r");
               }
               if(rc < 14){
                       vmerror("vionetrproc: short packet received (len=%d)", rc);
                       continue;
               }
               if(!viomacok(v, rxbuf))
                       continue;
               vb = viogetbuf(q, 1);
               if(vb == nil){
                       vmerror("viogetbuf: %r");
                       continue;
               }
               vioqwrite(vb, rxhead, sizeof(rxhead));
               vioqwrite(vb, rxbuf, rc);
               vioputbuf(vb);
       }
}

void
vionetwproc(void *vp)
{
       VIODev *v;
       VIOQueue *q;
       VIOBuf *vb;
       uchar txhead[10];
       uchar txbuf[1610];
       int rc, len;
       uvlong ns;

       threadsetname("vionetwproc");
       v = vp;
       q = &v->qu[1];
       for(;;){
               vb = viogetbuf(q, 1);
               if(vb == nil){
                       vmerror("viogetbuf: %r");
                       threadexits("viogetbuf: %r");
               }
               vioqread(vb, txhead, sizeof(txhead));
               len = vioqread(vb, txbuf+10, sizeof(txbuf)-10);
               if(len == sizeof(txbuf)-10){
                       vmerror("virtio net: ignoring excessively long packet");
                       vioputbuf(vb);
                       continue;
               }
               if(len < 14){
                       /* openbsd ends up sending lots of zero length packets sometimes */
                       if(len != 0)
                               vmerror("virtio net: ignoring short packet (length=%d)", len);
                       vioputbuf(vb);
                       continue;
               }else if(len < 60){ /* openbsd doesn't seem to know about ethernet minimum packet lengths either */
                       memset(txbuf + 10 + len, 0, 60 - len);
                       len = 60;
               }
               if((v->net.flags & VNETHEADER) != 0){
                       txbuf[0] = len  >> 8;
                       txbuf[1] = len;
                       ns = nsec();
                       txbuf[2] = ns >> 56;
                       txbuf[3] = ns >> 48;
                       txbuf[4] = ns >> 40;
                       txbuf[5] = ns >> 32;
                       txbuf[6] = ns >> 24;
                       txbuf[7] = ns >> 16;
                       txbuf[8] = ns >> 8;
                       txbuf[9] = ns;
                       rc = write(v->net.writefd, txbuf, len + 10);
               }else
                       rc = write(v->net.writefd, txbuf + 10, len);
               vioputbuf(vb);
               if(rc < 0){
                       vmerror("write(vionetwproc): %r");
                       continue;
               }
               if(rc < len){
                       vmerror("write(vionetwproc): incomplete write (%d < %d)", rc, len);
                       continue;
               }
       }
}

u32int
vionetio(int isin, u16int port, u32int val, int sz, VIODev *v)
{
       switch(isin << 16 | port){
       case 0x10000: case 0x10001: case 0x10002: case 0x10003:
               return GET32(v->net.mac, 0) >> (port & 3) * 8;
       case 0x10004: case 0x10005: case 0x10006: case 0x10007:
               return (GET16(v->net.mac, 4) | 1 << 16) >> (port & 3) * 8;
       }
       return iowhine(isin, port, val, sz, "virtio net");
}

int
vionettables(VIODev *d, VIOBuf *b)
{
       u8int buf[4];
       u8int mac[6];
       u64int bloom[2];
       int i, l;

       bloom[0] = 0;
       bloom[1] = 0;
       for(i = 0; i < 2; i++){
               if(vioqread(b, buf, 4) < 4)
                       return 1;
               l = GET32(buf, 0);
               while(l--){
                       if(vioqread(b, mac, 6) < 6)
                               return 1;
                       bloom[i] |= 1ULL<<bloomhash(mac);
               }
       }
       d->net.macbloom = bloom[0];
       d->net.multibloom = bloom[1];
       return 0;
}

void
vionetcmd(VIOQueue *q)
{
       VIODev *d;
       VIOBuf *b;
       u8int cmd[2], buf[6];
       u8int ack;
       int fl;

       d = q->d;
       for(; b = viogetbuf(q, 0), b != nil; vioputbuf(b)){
               if(vioqread(b, cmd, 2) < 2){
                       ack = 1;
                       vioqwrite(b, &ack, 1);
                       continue;
               }
               ack = 0;
               switch(cmd[0] << 8 | cmd[1]){
               case 0x0000: fl = VNETPROMISC; goto flag;
               case 0x0001: fl = VNETALLMULTI; goto flag;
               case 0x0002: fl = VNETALLUNI; goto flag;
               case 0x0003: fl = VNETNOMULTI; goto flag;
               case 0x0004: fl = VNETNOUNI; goto flag;
               case 0x0005: fl = VNETNOBCAST; goto flag;
               flag:
                       if(vioqread(b, buf, 1) < 1) ack = 1;
                       else if(buf[0] == 1) d->net.flags |= fl;
                       else if(buf[0] == 0) d->net.flags &= ~fl;
                       else ack = 1;
                       break;
               case 0x0100: /* MAC_TABLE_SET */
                       ack = vionettables(d, b);
                       break;
               case 0x0101: /* MAC_ADDR_SET */
                       if(vioqread(b, buf, 6) < 6) ack = 1;
                       else memmove(d->net.mac, buf, 6);
                       break;
               default:
                       ack = 1;
               }
               vioqwrite(b, &ack, 1);
       }
}

void
vionetreset(VIODev *d)
{
       d->net.flags &= VNETHEADER;
       d->net.macbloom = 0;
       d->net.multibloom = 0;
}

int
mkvionet(char *net)
{
       int fd, cfd;
       VIODev *d;
       char *ea;
       int flags;
       enum { VNETFILE = 1 };

       ea = nil;
       flags = 0;
       for(;;){
               if(strncmp(net, "hdr!", 4) == 0){
                       net += 4;
                       flags |= VNETHEADER;
               }else if(strncmp(net, "file!", 5) == 0){
                       net += 5;
                       flags |= VNETFILE;
               }else if(strncmp(net, "ea:", 3) == 0){
                       net = strchr(ea = net+3, '!');
                       if(net++ == nil){
                               werrstr("missing: !");
                               return -1;
                       }
               }else
                       break;
       }
       if((flags & VNETFILE) != 0){
               flags &= ~VNETFILE;
               fd = open(net, ORDWR);
               if(fd < 0) return -1;
       }else{
               fd = dial(netmkaddr("-1", net, nil), nil, nil, &cfd);
               if(fd < 0) return -1;
               if(cfd >= 0) {
                       write(cfd, "promiscuous", 11);
                       write(cfd, "bridge", 6);
               }
       }

       d = mkviodev(0x1000, 0x020000, 1, 3);
       mkvioqueue(d, 1024, viowakeup);
       mkvioqueue(d, 1024, viowakeup);
       mkvioqueue(d, 32, vionetcmd);
       if(ea == nil || parseether(d->net.mac, ea)){
               genrandom(d->net.mac, 6);
               d->net.mac[0] = d->net.mac[0] & ~1 | 2;
       }
       d->net.flags = flags;
       d->devfeat = 1<<5|1<<16|1<<17|1<<18|1<<20;
       d->io = vionetio;
       d->reset = vionetreset;
       d->net.readfd = d->net.writefd = fd;
       proccreate(vionetrproc, d, 8192);
       proccreate(vionetwproc, d, 8192);
       return 0;
}

u32int
vioblkio(int isin, u16int port, u32int val, int sz, VIODev *v)
{
       switch(isin << 16 | port){
       case 0x10000: case 0x10001: case 0x10002: case 0x10003:
               return (u32int)v->blk.size >> (port & 3) * 8;
       case 0x10004: case 0x10005: case 0x10006: case 0x10007:
               return (u32int)(v->blk.size >> 32) >> (port & 3) * 8;
       }
       return iowhine(isin, port, val, sz, "virtio blk");
}

void
vioblkproc(void *vp)
{
       VIODev *v;
       VIOQueue *q;
       VIOBuf *b;
       u8int cmd[16];
       u8int ack;
       char buf[8192];
       uvlong addr;
       int rc, n, m;

       threadsetname("vioblkproc");
       v = vp;
       q = &v->qu[0];
       for(;;){
               b = viogetbuf(q, 1);
               if(b == nil){
                       vmerror("vioblkproc: viogetbuf: %r");
                       threadexits("vioblkproc: viogetbuf: %r");
               }
               ack = 0;
               if(vioqread(b, cmd, sizeof(cmd)) < sizeof(cmd)) goto nope;
               addr = GET64(cmd, 8);
               switch(GET32(cmd, 0)){
               case 0:
                       n = vioqrem(b, 1) - 1;
                       if(n < 0 || addr * 512 + n > v->blk.size * 512){
                               ack = 1;
                               break;
                       }
                       seek(v->blk.fd, addr << 9, 0);
                       for(; n > 0; n -= rc){
                               rc = sizeof(buf);
                               if(n < rc) rc = n;
                               rc = read(v->blk.fd, buf, rc);
                               if(rc < 0) vmerror("read(vioblkproc): %r");
                               if(rc <= 0){
                                       ack = 1;
                                       break;
                               }
                               vioqwrite(b, buf, rc);
                       }
                       break;
               case 1:
                       n = vioqrem(b, 0);
                       if(addr * 512 + n > v->blk.size * 512){
                               ack = 1;
                               break;
                       }
                       seek(v->blk.fd, addr << 9, 0);
                       for(; n > 0; n -= rc){
                               m = vioqread(b, buf, sizeof(buf));
                               rc = write(v->blk.fd, buf, m);
                               if(rc < 0) vmerror("write(vioblkproc): %r");
                               if(rc < m){
                                       ack = 1;
                                       break;
                               }
                       }
                       break;
               default:
               nope:
                       ack = 2;
               }
               vioqwrite(b, &ack, 1);
               vioputbuf(b);
       }
}

int
mkvioblk(char *fn)
{
       int fd;
       VIODev *d;

       fd = open(fn, ORDWR);
       if(fd < 0) return -1;
       d = mkviodev(0x1000, 0x018000, 2, 1);
       mkvioqueue(d, 32, viowakeup);
       d->io = vioblkio;
       d->blk.fd = fd;
       d->blk.size = seek(fd, 0, 2) >> 9;
       proccreate(vioblkproc, d, 16384);
       return 0;
}

You are viewing proxied material from sdf.org. The copyright of proxied material belongs to its original authors. Any comments or complaints in relation to proxied material should be directed to the original authors of the content concerned. Please see the disclaimer for more details.