/*
* pad a block to the front (or the back if size is negative)
*/
struct block *padblock(struct block *bp, int size)
{
int n;
struct block *nbp;
uint8_t bcksum = bp->flag & BCKSUM_FLAGS;
uint16_t checksum_start = bp->checksum_start;
uint16_t checksum_offset = bp->checksum_offset;
uint16_t mss = bp->mss;
QDEBUG checkb(bp, "padblock 1");
if (size >= 0) {
if (bp->rp - bp->base >= size) {
bp->checksum_start += size;
bp->rp -= size;
return bp;
}
PANIC_EXTRA(bp);
if (bp->next)
panic("padblock %p", getcallerpc(&bp));
n = BLEN(bp);
padblockcnt++;
nbp = block_alloc(size + n, MEM_WAIT);
nbp->rp += size;
nbp->wp = nbp->rp;
memmove(nbp->wp, bp->rp, n);
nbp->wp += n;
freeb(bp);
nbp->rp -= size;
} else {
size = -size;
PANIC_EXTRA(bp);
if (bp->next)
panic("padblock %p", getcallerpc(&bp));
if (bp->lim - bp->wp >= size)
return bp;
n = BLEN(bp);
padblockcnt++;
nbp = block_alloc(size + n, MEM_WAIT);
memmove(nbp->wp, bp->rp, n);
nbp->wp += n;
freeb(bp);
}
if (bcksum) {
nbp->flag |= bcksum;
nbp->checksum_start = checksum_start;
nbp->checksum_offset = checksum_offset;
nbp->mss = mss;
}
QDEBUG checkb(nbp, "padblock 1");
return nbp;
}
/*
* copy 'count' bytes into a new block
*/
struct block *copyblock(struct block *bp, int count)
{
int l;
struct block *nbp;
QDEBUG checkb(bp, "copyblock 0");
nbp = block_alloc(count, MEM_WAIT);
if (bp->flag & BCKSUM_FLAGS) {
nbp->flag |= (bp->flag & BCKSUM_FLAGS);
nbp->checksum_start = bp->checksum_start;
nbp->checksum_offset = bp->checksum_offset;
nbp->mss = bp->mss;
}
PANIC_EXTRA(bp);
for (; count > 0 && bp != 0; bp = bp->next) {
l = BLEN(bp);
if (l > count)
l = count;
memmove(nbp->wp, bp->rp, l);
nbp->wp += l;
count -= l;
}
if (count > 0) {
memset(nbp->wp, 0, count);
nbp->wp += count;
}
copyblockcnt++;
QDEBUG checkb(nbp, "copyblock 1");
return nbp;
}
/*
* copy from offset in the queue
*/
struct block *qcopy_old(struct queue *q, int len, uint32_t offset)
{
int sofar;
int n;
struct block *b, *nb;
uint8_t *p;
nb = block_alloc(len, MEM_WAIT);
spin_lock_irqsave(&q->lock);
/* go to offset */
b = q->bfirst;
for (sofar = 0;; sofar += n) {
if (b == NULL) {
spin_unlock_irqsave(&q->lock);
return nb;
}
n = BLEN(b);
if (sofar + n > offset) {
p = b->rp + offset - sofar;
n -= offset - sofar;
break;
}
QDEBUG checkb(b, "qcopy");
b = b->next;
}
/* copy bytes from there */
for (sofar = 0; sofar < len;) {
if (n > len - sofar)
n = len - sofar;
PANIC_EXTRA(b);
memmove(nb->wp, p, n);
qcopycnt += n;
sofar += n;
nb->wp += n;
b = b->next;
if (b == NULL)
break;
n = BLEN(b);
p = b->rp;
}
spin_unlock_irqsave(&q->lock);
return nb;
}
/*
* get next block from a queue (up to a limit)
*/
struct block *qbread(struct queue *q, int len)
{
ERRSTACK(1);
struct block *b, *nb;
int n;
qlock(&q->rlock);
if (waserror()) {
qunlock(&q->rlock);
nexterror();
}
spin_lock_irqsave(&q->lock);
if (!qwait(q)) {
/* queue closed */
spin_unlock_irqsave(&q->lock);
qunlock(&q->rlock);
poperror();
return NULL;
}
/* if we get here, there's at least one block in the queue */
b = qremove(q);
n = BLEN(b);
/* split block if it's too big and this is not a message queue */
nb = b;
if (n > len) {
PANIC_EXTRA(b);
if ((q->state & Qmsg) == 0) {
n -= len;
b = allocb(n);
memmove(b->wp, nb->rp + len, n);
b->wp += n;
qputback(q, b);
}
nb->wp = nb->rp + len;
}
/* restart producer */
qwakeup_iunlock(q);
poperror();
qunlock(&q->rlock);
return nb;
}
/*
* copy the string of blocks into
* a single block and free the string
*/
struct block *concatblock(struct block *bp)
{
int len;
struct block *nb, *f;
if (bp->next == 0)
return bp;
/* probably use parts of qclone */
PANIC_EXTRA(bp);
nb = block_alloc(blocklen(bp), MEM_WAIT);
for (f = bp; f; f = f->next) {
len = BLEN(f);
memmove(nb->wp, f->rp, len);
nb->wp += len;
}
concatblockcnt += BLEN(nb);
freeblist(bp);
QDEBUG checkb(nb, "concatblock 1");
return nb;
}
int qproduce(struct queue *q, void *vp, int len)
{
struct block *b;
int dowakeup;
uint8_t *p = vp;
/* sync with qread */
dowakeup = 0;
spin_lock_irqsave(&q->lock);
/* no waiting receivers, room in buffer? */
if (q->len >= q->limit) {
q->state |= Qflow;
spin_unlock_irqsave(&q->lock);
return -1;
}
/* save in buffer */
/* use Qcoalesce here to save storage */
// TODO: Consider removing the Qcoalesce flag and force a coalescing
// strategy by default.
b = q->blast;
if ((q->state & Qcoalesce) == 0 || q->bfirst == NULL
|| b->lim - b->wp < len) {
/* need a new block */
b = iallocb(len);
if (b == 0) {
spin_unlock_irqsave(&q->lock);
return 0;
}
if (q->bfirst)
q->blast->next = b;
else
q->bfirst = b;
q->blast = b;
/* b->next = 0; done by iallocb() */
q->len += BALLOC(b);
}
PANIC_EXTRA(b);
memmove(b->wp, p, len);
producecnt += len;
b->wp += len;
q->dlen += len;
QDEBUG checkb(b, "qproduce");
if (q->state & Qstarve) {
q->state &= ~Qstarve;
dowakeup = 1;
}
if (q->len >= q->limit)
q->state |= Qflow;
spin_unlock_irqsave(&q->lock);
if (dowakeup) {
rendez_wakeup(&q->rr);
qwake_cb(q, FDTAP_FILT_READABLE);
}
return len;
}
/*
* Interrupt level copy out of a queue, return # bytes copied.
*/
int qconsume(struct queue *q, void *vp, int len)
{
struct block *b;
int n, dowakeup;
uint8_t *p = vp;
struct block *tofree = NULL;
/* sync with qwrite */
spin_lock_irqsave(&q->lock);
for (;;) {
b = q->bfirst;
if (b == 0) {
q->state |= Qstarve;
spin_unlock_irqsave(&q->lock);
return -1;
}
QDEBUG checkb(b, "qconsume 1");
n = BLEN(b);
if (n > 0)
break;
q->bfirst = b->next;
q->len -= BALLOC(b);
/* remember to free this */
b->next = tofree;
tofree = b;
};
PANIC_EXTRA(b);
if (n < len)
len = n;
memmove(p, b->rp, len);
consumecnt += n;
b->rp += len;
q->dlen -= len;
/* discard the block if we're done with it */
if ((q->state & Qmsg) || len == n) {
q->bfirst = b->next;
b->next = 0;
q->len -= BALLOC(b);
q->dlen -= BLEN(b);
/* remember to free this */
b->next = tofree;
tofree = b;
}
/* if writer flow controlled, restart */
if ((q->state & Qflow) && q->len < q->limit / 2) {
q->state &= ~Qflow;
dowakeup = 1;
} else
dowakeup = 0;
spin_unlock_irqsave(&q->lock);
if (dowakeup) {
rendez_wakeup(&q->wr);
qwake_cb(q, FDTAP_FILT_WRITABLE);
}
if (tofree != NULL)
freeblist(tofree);
return len;
}
