/* * 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; }