static void *
writer(void *aport)
{
Port *port = (Port *)aport;
Buffer *bp;
int nwr, nref;
for(;;){
bp = qget(&port->xmitq);
if(bp == NULL)
break;
if(bp->len > 0){
*(uint32_t *)bp->buf = 0;
nwr = write(port->fd, bp->buf, bp->len);
if(nwr != bp->len){
fprintf(stderr, "%s: short write, got %d wanted %d\n", portname(port), nwr, bp->len);
break;
}
}
nref = bdecref(bp);
if(nref == 0)
qput(bp->freeq, bp);
}
fprintf(stderr, "%s: writer exiting\n", portname(port));
return port;
}
Block*
netifbread(Netif *nif, Chan *c, long n, vlong offset)
{
Netfile *f;
Block *bp;
if((c->qid.type & QTDIR) || NETTYPE(c->qid.path) != Ndataqid)
return devbread(c, n, offset);
f = nif->f[NETID(c->qid.path)];
if(f->fat){
/*
* Frame at a time (fat) allows us to provide
* non-blocking performance with blocking semantics
* for consumers that know ahead of time data is
* contained within a single frame. Once empty, we
* get in line with other blocking reads and wait our
* turn.
*/
for(;;){
if(bp = qget(f->iq))
return bp;
if(waserror())
return nil;
qsleep(f->iq);
poperror();
}
}
return qbread(f->iq, n);
}
/*
* called when a process writes to an interface's 'data'
*/
static void
ipifckick(void *x)
{
Proc *up = externup();
Conv *c = x;
Block *bp;
Ipifc *ifc;
bp = qget(c->wq);
if(bp == nil)
return;
ifc = (Ipifc*)c->ptcl;
if(!canrlock(ifc)){
freeb(bp);
return;
}
if(waserror()){
runlock(ifc);
nexterror();
}
if(ifc->medium == nil || ifc->medium->pktin == nil)
freeb(bp);
else
(*ifc->medium->pktin)(c->p->f, ifc, bp);
runlock(ifc);
poperror();
}
static void *
reader(void *aport)
{
Buffer *bp;
Port *port;
Cam *cam;
uint8_t *dstmac, *srcmac;
int i, nrd, nref;
port = (Port *)aport;
for(;;){
bp = qget(&port->freeq);
nrd = read(port->fd, bp->buf, bp->cap);
if(port->state != PortOpen){
qput(bp->freeq, bp);
break;
}
bp->len = nrd;
dstmac = (uint8_t *)bp->buf + 4;
srcmac = (uint8_t *)bp->buf + 10;
nref = 0;
cam = camlook(g_cams, dstmac);
if(cam != NULL && cam->port != NULL){
// port found in cam, forward only there...
nref = bincref(bp);
if(qput(&cam->port->xmitq, bp) == -1)
nref = bdecref(bp);
} else {
// broadcast..
for(i = 0; i < nports; i++){
if(port == (ports+i))
continue;
nref = bincref(bp);
if(qput(&ports[i].xmitq, bp) == -1)
nref = bdecref(bp);
}
}
// teach the switch about the source address we just saw
cam = camlook(g_cams, srcmac);
if(cam != NULL){
// always update the port, so if an address moves to a different port
// the cam will point to that port right away.
copymac(cam->mac, srcmac);
cam->age = 0;
cam->port = port;
} else {
fprintf(stderr, "cam presumably full..\n");
}
// ref is zero after the forward loop. it didn't go anywhere, so drop it.
if(nref == 0)
qput(bp->freeq, bp);
}
fprintf(stderr, "%s: reader exiting..\n", portname(port));
return port;
}
int main(int argc, char * argv[]){
int cclients = 10;
int sentMsg = 0;
printf("Server iniciado para %d clientes.\n", cclients);
message_t incomingMsg;
message_t msgsent;
char * word = "Mensaje del Server";
ipc_t server = (ipc_t) fifoServe(cclients);
server->stop = 0;
while(sentMsg != 1000){
if((incomingMsg = qget(server->inbox)) != NULL){
printf("%d. Server recibio: %s\n", sentMsg, incomingMsg->data);
printf("Manda mensaje a:%d.\n", incomingMsg->header.from);
qput(server->outbox, (msgsent = mnew(0,incomingMsg->header.from, strlen(word) + 1, word)));
mdel(msgsent);
mdel(incomingMsg);
sentMsg++;
}
}
stopServer(server);
}
void * mq_clientLoop(void* ipcarg)
{
ipc_t ipc;
ipc = (ipc_t) ipcarg;
message_t msg;
if(ipc->status == IPCSTAT_DISCONNECTED)
{
return NULL;
}
ipc->status = IPCSTAT_CONNECTED;
while (ipc->stop != 1) {
msg = mq_getData(ipc, ipc->ipcdata->queuedata.recvPrior);
if(msg != NULL)
{
qput(ipc->inbox, msg);
}
msg = qget(ipc->outbox);
if(msg != NULL)
{
mq_sendData(ipc,msg,SERVERKEY);
}
}
mq_disconnect(ipc);
free(msg);
return NULL;
}
/*
* called when a process writes to an interface's 'data'
*/
static void ipifckick(void *x)
{
ERRSTACK(1);
struct conv *c = x;
struct block *bp;
struct Ipifc *ifc;
bp = qget(c->wq);
if (bp == NULL)
return;
ifc = (struct Ipifc *)c->ptcl;
if (!canrlock(&ifc->rwlock)) {
freeb(bp);
return;
}
if (waserror()) {
runlock(&ifc->rwlock);
nexterror();
}
if (ifc->m == NULL || ifc->m->pktin == NULL)
freeb(bp);
else
(*ifc->m->pktin) (c->p->f, ifc, bp);
runlock(&ifc->rwlock);
poperror();
}
int
etherrxpkt(int ctlrno, Etherpkt *pkt, int timo)
{
int n;
Ctlr *ctlr;
Block *b;
ulong start;
if((ctlr = attach(ctlrno)) == 0)
return 0;
start = m->ticks;
while((b = qget(ctlr->iq)) == 0){
if(TK2MS(m->ticks - start) >= timo){
/*
print("ether%d: rx timeout\n", ctlrno);
*/
return 0;
}
//delay(1);
}
n = BLEN(b);
memmove(pkt, b->rp, n);
freeb(b);
return n;
}
/*
* transmit strategy: fill the output ring as far as possible,
* perhaps leaving a few spare; kick off the output and take
* an interrupt only when the transmit queue is empty.
*/
static void
transmit(Ether *ether)
{
int i, kick, len;
Block *b;
Ctlr *ctlr = ether->ctlr;
Gbereg *reg = ctlr->reg;
Tx *t;
ethercheck(ether);
ilock(ctlr);
txreplenish(ether); /* reap old packets */
/* queue new packets; use at most half the tx descs to avoid livelock */
kick = 0;
for (i = Ntx/2 - 2; i > 0; i--) {
t = &ctlr->tx[ctlr->txhead]; /* *t is uncached */
assert(((uintptr)t & (Descralign - 1)) == 0);
if(t->cs & TCSdmaown) { /* descriptor busy? */
ctlr->txringfull++;
break;
}
b = qget(ether->oq); /* outgoing packet? */
if (b == nil)
break;
len = BLEN(b);
if(len < ether->minmtu || len > ether->maxmtu) {
freeb(b);
continue;
}
ctlr->txb[ctlr->txhead] = b;
/* make sure the whole packet is in memory */
cachedwbse(b->rp, len);
l2cacheuwbse(b->rp, len);
/* set up the transmit descriptor */
t->buf = PADDR(b->rp);
t->countchk = len << 16;
coherence();
/* and fire */
t->cs = TCSpadding | TCSfirst | TCSlast | TCSdmaown |
TCSenableintr;
coherence();
kick++;
ctlr->txhead = NEXT(ctlr->txhead, Ntx);
}
if (kick) {
txkick(ctlr);
reg->irqmask |= Itxendq(Qno);
reg->irqemask |= IEtxerrq(Qno) | IEtxunderrun;
}
iunlock(ctlr);
}
static void
txstart(Ether* ether)
{
int port;
Smc91xx* ctlr;
Block* bp;
int len, npages;
int pno;
/* assumes ctlr is locked and bank 2 is selected */
/* leaves bank 2 selected on return */
port = ether->port;
ctlr = ether->ctlr;
if (ctlr->txbp) {
bp = ctlr->txbp;
ctlr->txbp = 0;
} else {
bp = qget(ether->oq);
if (bp == 0)
return;
len = BLEN(bp);
npages = (len + HdrSize) / PageSize;
outs(port + MmuCmd, McAlloc | npages);
}
pno = inb(port + AllocRes);
if (pno & ArFailed) {
outb(port + IntrMask, inb(port + IntrMask) | IntAlloc);
ctlr->txbp = bp;
ctlr->txtime = MACHP(0)->ticks;
return;
}
outb(port + PktNo, pno);
outs(port + Pointer, PtrAutoInc);
len = BLEN(bp);
outs(port + Data1, 0);
outb(port + Data1, (len + HdrSize) & 0xFF);
outb(port + Data1, (len + HdrSize) >> 8);
outss(port + Data1, bp->rp, len / 2);
if ((len & 1) == 0) {
outs(port + Data1, 0);
} else {
outb(port + Data1, bp->rp[len - 1]);
outb(port + Data1, 0x20); /* no info what 0x20 means */
}
outb(port + IntrMask, inb(port + IntrMask) |
IntTxError | IntTxEmpty);
outs(port + MmuCmd, McEnqueue);
freeb(bp);
}
/* runs in its own thread so the main thread won't block */
void *fd_log(void *data){
char *line;
while(1){
while( (line = (char *) qget(log_queue)) != NULL ){
fprintf(log_file, line);
fflush(log_file);
}
}
}
static void traverse(STri *t, int op) {
STri tnew,tr;
qinit();
qput(t,op);
while(qany()) {
op = qget(&tr);
if (!reg_stri(&tr)) continue;
if (op!=Q_OP) { Q_refl(&tr,&tnew); qput(&tnew,Q_OP); }
if (op!=P_OP) { P_refl(&tr,&tnew); qput(&tnew,P_OP); }
if (op!=R_OP) { R_refl(&tr,&tnew); qput(&tnew,R_OP); }
}
}
static void
w_txstart(Ether* ether)
{
Etherpkt *pkt;
Ctlr *ctlr;
Block *bp;
int len, off;
if((ctlr = ether->ctlr) == nil || (ctlr->state & (Attached|Power)) != (Attached|Power) || ctlr->txbusy)
return;
if((bp = qget(ether->oq)) == nil)
return;
pkt = (Etherpkt*)bp->rp;
//
// If the packet header type field is > 1500 it is an IP or
// ARP datagram, otherwise it is an 802.3 packet. See RFC1042.
//
memset(&ctlr->txf, 0, sizeof(ctlr->txf));
if(((pkt->type[0]<<8)|pkt->type[1]) > 1500){
ctlr->txf.framectl = WF_Data;
memmove(ctlr->txf.addr1, pkt->d, Eaddrlen);
memmove(ctlr->txf.addr2, pkt->s, Eaddrlen);
memmove(ctlr->txf.dstaddr, pkt->d, Eaddrlen);
memmove(ctlr->txf.srcaddr, pkt->s, Eaddrlen);
memmove(&ctlr->txf.type, pkt->type, 2);
bp->rp += ETHERHDRSIZE;
len = BLEN(bp);
off = WF_802_11_Off;
ctlr->txf.dlen = len+ETHERHDRSIZE-WSnapHdrLen;
hnputs((uchar*)&ctlr->txf.dat[0], WSnap0);
hnputs((uchar*)&ctlr->txf.dat[1], WSnap1);
hnputs((uchar*)&ctlr->txf.len, len+ETHERHDRSIZE-WSnapHdrLen);
}
else{
len = BLEN(bp);
off = WF_802_3_Off;
ctlr->txf.dlen = len;
}
w_write(ctlr, ctlr->txdid, 0, &ctlr->txf, sizeof(ctlr->txf));
w_write(ctlr, ctlr->txdid, off, bp->rp, len+2);
if(w_cmd(ctlr, WCmdReclaim|WCmdTx, ctlr->txdid)){
DEBUG("wavelan: transmit failed\n");
ctlr->ntxerr++;
}
else{
ctlr->txbusy = 1;
ctlr->txtmout = 2;
}
freeb(bp);
}
static int qget_uint64(leveldb::DB* db, const Bytes &name, uint64_t seq, uint64_t *ret){
std::string val;
*ret = 0;
int s = qget(db, name, seq, &val);
if(s == 1){
if(val.size() != sizeof(uint64_t)){
return -1;
}
*ret = *(uint64_t *)val.data();
}
return s;
}
void* lqget(void *qp){
if(NULL != qp){
pthread_mutex_t *q_lock = &(((lqtype *)qp)->q_lock);
int rc = pthread_mutex_lock(q_lock);
void * ret = qget(((lqtype *)qp)->queue);
rc = pthread_mutex_unlock(q_lock);
return ret;
} else {
printf("should open queue before using it\n");
return NULL;
}
}
// @return 0: empty queue, 1: item peeked, -1: error
int SSDB::qfront(const Bytes &name, std::string *item){
int ret = 0;
uint64_t seq;
ret = qget_uint64(this->db, name, QFRONT_SEQ, &seq);
if(ret == -1){
return -1;
}
if(ret == 0){
return 0;
}
ret = qget(this->db, name, seq, item);
return ret;
}
/*
* application entry point
*
*/
int main(int argc, char *argv[])
{
char *p;
struct queue *q = qalloc();
while (argc--) qput(q, *argv++);
while ((p = qget(q))) printf("%s\n", p);
qfree(q);
return 0;
}
// @return 0: empty queue, 1: item popped, -1: error
int SSDB::qpop(const Bytes &name, std::string *item){
Transaction trans(binlogs);
int ret;
uint64_t seq;
ret = qget_uint64(this->db, name, QFRONT_SEQ, &seq);
if(ret == -1){
return -1;
}
if(ret == 0){
seq = QITEM_MIN_SEQ;
}
ret = qget(this->db, name, seq, item);
if(ret == -1){
return -1;
}
if(ret == 0){
return 0;
}
// delete item
ret = qdel_one(this, name, seq);
if(ret == -1){
return -1;
}
// update size
int64_t size = incr_qsize(this, name, -1);
if(size == -1){
return -1;
}
// update front
if(size > 0){
seq += 1;
//log_debug("seq: %" PRIu64 ", ret: %d", seq, ret);
ret = qset_one(this, name, QFRONT_SEQ, Bytes(&seq, sizeof(seq)));
if(ret == -1){
return -1;
}
}
leveldb::Status s = binlogs->commit();
if(!s.ok()){
log_error("Write error!");
return -1;
}
return 1;
}
void * mq_serverLoop(void* ipcarg)
{
ipc_t ipc;
ipc = (ipc_t) ipcarg;
message_t msg;
if(ipc->status == IPCSTAT_DISCONNECTED)
{
return NULL;
}
ipc->status = IPCSTAT_SERVING;
while (ipc->stop != 1) {
msg = mq_getData(ipc, SERVERKEY);
if(msg != NULL)
{
if(msg->header.to == SERVERKEY)
{
/*
//[TODO] ver que pasa si hay un mensaje con to = 0
//pero hay que cambiar el to.. 0 no existe*/
/* Es esto lo que tiene que hacer?*/
qput(ipc->inbox, msg);
}
else
{
mq_sendData(ipc,msg,msg->header.to);
}
}
msg = qget(ipc->outbox);
if(msg != NULL)
{
mq_sendData(ipc,msg,SERVERKEY);
}
}
mq_disconnect(ipc);
free(msg);
return NULL;
}
static void
txstart(Ether *ether)
{
int len;
Ctlr *ctlr;
Block *b;
BD *dre;
ctlr = ether->ctlr;
if(ctlr->init)
return;
while(ctlr->ntq < Ntdre-1){
b = qget(ether->oq);
if(b == 0)
break;
dre = &ctlr->tdr[ctlr->tdrh];
dczap(dre, sizeof(BD));
if(dre->status & BDReady)
panic("ether: txstart");
/*
* Give ownership of the descriptor to the chip, increment the
* software ring descriptor pointer and tell the chip to poll.
*/
len = BLEN(b);
if(ctlr->txb[ctlr->tdrh] != nil)
panic("fcc/ether: txstart");
ctlr->txb[ctlr->tdrh] = b;
if((ulong)b->rp&1)
panic("fcc/ether: txstart align"); /* TO DO: ensure alignment */
dre->addr = PADDR(b->rp);
dre->length = len;
dcflush(b->rp, len);
dcflush(dre, sizeof(BD));
dre->status = (dre->status & BDWrap) | BDReady|TxPad|BDInt|BDLast|TxTC;
dcflush(dre, sizeof(BD));
/* ctlr->fcc->ftodr = 1<<15; /* transmit now; Don't do this according to errata */
ctlr->ntq++;
ctlr->tdrh = NEXT(ctlr->tdrh, Ntdre);
}
}
static void *cruncher(void *args) {
double val;
int f_depth;
QueueElement_t *qe;
CruncherInput_t *input = (CruncherInput_t *)args;
// Wait loop
while (!(output.calc_complete && !input->queue->num_elements)) {
// CRITICAL SECTION
// Grab element
pthread_mutex_lock(&q_lock);
qe = (QueueElement_t *)qget(input->queue->qp);
// END CRITICAL SECTION
if (qe == NULL) {
pthread_mutex_unlock(&q_lock);
sleep(CRUNCHER_WAIT_TIME);
continue;
}
else {
input->queue->num_elements--;
//printf("Item taken from queue, queue now holds %d\n", input->queue->num_elements);
//fflush(stdout);
pthread_mutex_unlock(&q_lock);
}
// Crunch division
val = integrate(qe->fp, qe->a, qe->b, qe->p, 0, 0, &f_depth);
// Grab lock
pthread_mutex_lock(&a_lock);
// CRITICAL SECTION
// Add result to accumulator
output.accumulator += val;
output.depth += f_depth;
pthread_mutex_unlock(&a_lock);
// END CRITICAL SECTION
}
return NULL;
}
static void
txfill(Ether*ether, Ctlr*ctlr)
{
int len;
Block *b;
ushort*dst;
while(ctlr->ntx<Ntx){
if((b=qget(ether->oq)) == nil)
break;
len = BLEN(b);
dst = (ushort*)(Ethermem+(ctlr->txempty+Nrx)*Etherfsize);
*dst = len;
memmove(&dst[1], b->rp, len);
ctlr->ntx++;
ctlr->txempty++;
if(ctlr->txempty==Ntx)
ctlr->txempty = 0;
freeb(b);
}
}
static void
txstart(Ether* ether)
{
Ctlr *ctlr;
Block *bp;
Des *des;
int control;
ctlr = ether->ctlr;
while(ctlr->ntq < (ctlr->ntdr-1)){
if(ctlr->setupbp){
bp = ctlr->setupbp;
ctlr->setupbp = 0;
control = Ic|Set|BLEN(bp);
}
else{
bp = qget(ether->oq);
if(bp == nil)
break;
control = Ic|Lseg|Fseg|BLEN(bp);
}
ctlr->tdr[PREV(ctlr->tdrh, ctlr->ntdr)].control &= ~Ic;
des = &ctlr->tdr[ctlr->tdrh];
des->bp = bp;
des->addr = PCIWADDR(bp->rp);
des->control |= control;
ctlr->ntq++;
coherence();
des->status = Own;
csr32w(ctlr, 1, 0);
ctlr->tdrh = NEXT(ctlr->tdrh, ctlr->ntdr);
}
if(ctlr->ntq > ctlr->ntqmax)
ctlr->ntqmax = ctlr->ntq;
}
/* Consumer thread, take from queue and execute user callback */
static void *recthread2(void *arg)
{
header *hdr;
signal *sig;
char *msg;
for (;;) {
msg = qget(queue_index);
hdr = (header *) msg;
print(LOG_INFO, "Taking msg from queue with msg_type %d\n",
hdr->msg_type);
switch (hdr->msg_type) {
case DATA:
if (callbackdata != NULL) {
callbackdata(hdr->proc_name,
(msg + SIZEOF_HEADER),
hdr->msg_len);
free(msg);
} else {
print(LOG_WARNING,
"No callback for msg_type %d\n", DATA);
}
break;
case SIGNAL1:
sig = (signal *) (msg + SIZEOF_HEADER);
if (callback1 != NULL) {
callback1(hdr->proc_name, sig->signal1);
free(msg);
} else {
print(LOG_WARNING,
"No callback for msg_type %d\n", SIGNAL1);
}
break;
case SIGNAL2:
sig = (signal *) (msg + SIZEOF_HEADER);
if (callback2 != NULL) {
callback2(hdr->proc_name, sig->signal1,
sig->signal2);
free(msg);
} else {
print(LOG_WARNING,
"No callback for msg_type %d\n", SIGNAL2);
}
break;
case SIGNAL3:
sig = (signal *) (msg + SIZEOF_HEADER);
if (callback3 != NULL) {
callback3(hdr->proc_name, sig->signal1,
sig->signal2, sig->signal3);
free(msg);
} else {
print(LOG_WARNING,
"No callback for msg_type %d\n", SIGNAL3);
}
break;
default:
print(LOG_ERR, "Illeagal msg_type %d\n", hdr->msg_type);
free(msg);
break;
}
}
return (void *)0;
}
/*
* move blocks between queues if they are ready.
* schedule an interrupt for the next interesting time.
*
* must be called with the link ilocked.
*/
static void
pushlink(Link *link, vlong now)
{
Block *bp;
vlong tout, tin;
/*
* put another block in the link queue
*/
ilock(link);
if(link->iq == nil || link->oq == nil){
iunlock(link);
return;
}
timerdel(&link->ci);
/*
* put more blocks into the xmit queue
* use the time the last packet was supposed to go out
* as the start time for the next packet, rather than
* the current time. this more closely models a network
* device which can queue multiple output packets.
*/
tout = link->tout;
if(!tout)
tout = now;
while(tout <= now){
bp = qget(link->oq);
if(bp == nil){
tout = 0;
break;
}
/*
* can't send the packet before it gets queued
*/
tin = gtime(bp->rp);
if(tin > tout)
tout = tin;
tout = tout + (BLEN(bp) - Tmsize) * link->delayn;
/*
* drop packets
*/
if(link->droprate && nrand(link->droprate) == 0)
link->drops++;
else{
ptime(bp->rp, tout + link->delay0ns);
if(link->tq == nil)
link->tq = bp;
else
link->tqtail->next = bp;
link->tqtail = bp;
}
}
/*
* record the next time a packet can be sent,
* but don't schedule an interrupt if none is waiting
*/
link->tout = tout;
if(!qcanread(link->oq))
tout = 0;
/*
* put more blocks into the receive queue
*/
tin = 0;
while(bp = link->tq){
tin = gtime(bp->rp);
if(tin > now)
break;
bp->rp += Tmsize;
link->tq = bp->next;
bp->next = nil;
if(!link->indrop)
qpassnolim(link->iq, bp);
else if(qpass(link->iq, bp) < 0)
link->soverflows++;
tin = 0;
}
if(bp == nil && qisclosed(link->oq) && !qcanread(link->oq) && !qisclosed(link->iq))
qhangup(link->iq, nil);
link->tin = tin;
if(!tin || tin > tout && tout)
tin = tout;
link->ci.ns = tin - now;
if(tin){
if(tin < now)
panic("loopback unfinished business");
timeradd(&link->ci);
}
iunlock(link);
}
static void
espkick(void *x)
{
Conv *c = x;
Esphdr *eh;
Esptail *et;
Userhdr *uh;
Espcb *ecb;
Block *bp;
int nexthdr;
int payload;
int pad;
int align;
uchar *auth;
bp = qget(c->wq);
if(bp == nil)
return;
qlock(c);
ecb = c->ptcl;
if(ecb->header) {
/* make sure the message has a User header */
bp = pullupblock(bp, UserhdrSize);
if(bp == nil) {
qunlock(c);
return;
}
uh = (Userhdr*)bp->rp;
nexthdr = uh->nexthdr;
bp->rp += UserhdrSize;
} else {
nexthdr = 0; // what should this be?
}
payload = BLEN(bp) + ecb->espivlen;
/* Make space to fit ip header */
bp = padblock(bp, EsphdrSize + ecb->espivlen);
align = 4;
if(ecb->espblklen > align)
align = ecb->espblklen;
if(align % ecb->ahblklen != 0)
panic("espkick: ahblklen is important after all");
pad = (align-1) - (payload + EsptailSize-1)%align;
/*
* Make space for tail
* this is done by calling padblock with a negative size
* Padblock does not change bp->wp!
*/
bp = padblock(bp, -(pad+EsptailSize+ecb->ahlen));
bp->wp += pad+EsptailSize+ecb->ahlen;
eh = (Esphdr *)(bp->rp);
et = (Esptail*)(bp->rp + EsphdrSize + payload + pad);
// fill in tail
et->pad = pad;
et->nexthdr = nexthdr;
ecb->cipher(ecb, bp->rp+EsphdrSize, payload+pad+EsptailSize);
auth = bp->rp + EsphdrSize + payload + pad + EsptailSize;
// fill in head
eh->vihl = IP_VER4;
hnputl(eh->espspi, ecb->spi);
hnputl(eh->espseq, ++ecb->seq);
v6tov4(eh->espsrc, c->laddr);
v6tov4(eh->espdst, c->raddr);
eh->espproto = IP_ESPPROTO;
eh->frag[0] = 0;
eh->frag[1] = 0;
ecb->auth(ecb, bp->rp+IphdrSize, (EsphdrSize-IphdrSize)+payload+pad+EsptailSize, auth);
qunlock(c);
//print("esp: pass down: %uld\n", BLEN(bp));
ipoput4(c->p->f, bp, 0, c->ttl, c->tos, c);
}
/*
* encapsulate next IP packet on x's write queue in IP/ESP packet
* and initiate output of the result.
*/
static void
espkick(void *x)
{
int nexthdr, payload, pad, align;
uint8_t *auth;
Block *bp;
Conv *c = x;
Esp4hdr *eh4;
Esp6hdr *eh6;
Espcb *ecb;
Esptail *et;
Userhdr *uh;
Versdep vers;
getverslens(convipvers(c), &vers);
bp = qget(c->wq);
if(bp == nil)
return;
qlock(c);
ecb = c->ptcl;
if(ecb->header) {
/* make sure the message has a User header */
bp = pullupblock(bp, Userhdrlen);
if(bp == nil) {
qunlock(c);
return;
}
uh = (Userhdr*)bp->rp;
nexthdr = uh->nexthdr;
bp->rp += Userhdrlen;
} else {
nexthdr = 0; /* what should this be? */
}
payload = BLEN(bp) + ecb->espivlen;
/* Make space to fit ip header */
bp = padblock(bp, vers.hdrlen + ecb->espivlen);
getpktspiaddrs(bp->rp, &vers);
align = 4;
if(ecb->espblklen > align)
align = ecb->espblklen;
if(align % ecb->ahblklen != 0)
panic("espkick: ahblklen is important after all");
pad = (align-1) - (payload + Esptaillen-1)%align;
/*
* Make space for tail
* this is done by calling padblock with a negative size
* Padblock does not change bp->wp!
*/
bp = padblock(bp, -(pad+Esptaillen+ecb->ahlen));
bp->wp += pad+Esptaillen+ecb->ahlen;
et = (Esptail*)(bp->rp + vers.hdrlen + payload + pad);
/* fill in tail */
et->pad = pad;
et->nexthdr = nexthdr;
/* encrypt the payload */
ecb->cipher(ecb, bp->rp + vers.hdrlen, payload + pad + Esptaillen);
auth = bp->rp + vers.hdrlen + payload + pad + Esptaillen;
/* fill in head; construct a new IP header and an ESP header */
if (vers.version == V4) {
eh4 = (Esp4hdr *)bp->rp;
eh4->vihl = IP_VER4;
v6tov4(eh4->espsrc, c->laddr);
v6tov4(eh4->espdst, c->raddr);
eh4->espproto = IP_ESPPROTO;
eh4->frag[0] = 0;
eh4->frag[1] = 0;
hnputl(eh4->espspi, ecb->spi);
hnputl(eh4->espseq, ++ecb->seq);
} else {
eh6 = (Esp6hdr *)bp->rp;
eh6->vcf[0] = IP_VER6;
ipmove(eh6->src, c->laddr);
ipmove(eh6->dst, c->raddr);
eh6->proto = IP_ESPPROTO;
hnputl(eh6->espspi, ecb->spi);
hnputl(eh6->espseq, ++ecb->seq);
}
/* compute secure hash */
ecb->auth(ecb, bp->rp + vers.iphdrlen, (vers.hdrlen - vers.iphdrlen) +
payload + pad + Esptaillen, auth);
qunlock(c);
/* print("esp: pass down: %uld\n", BLEN(bp)); */
if (vers.version == V4)
ipoput4(c->p->f, bp, 0, c->ttl, c->tos, c);
else
ipoput6(c->p->f, bp, 0, c->ttl, c->tos, c);
}
static void
vt6102transmit(Ether* edev)
{
Block *bp;
Ctlr *ctlr;
Ds *ds, *next;
int control, i, o, prefix, size, tdused, timeo;
ctlr = edev->ctlr;
ilock(&ctlr->tlock);
/*
* Free any completed packets
*/
ds = ctlr->tdh;
for(tdused = ctlr->tdused; tdused > 0; tdused--){
/*
* For some errors the chip will turn the Tx engine
* off. Wait for that to happen.
* Could reset and re-init the chip here if it doesn't
* play fair.
* To do: adjust Tx FIFO threshold on underflow.
*/
if(ds->status & (Abt|Tbuff|Udf)){
for(timeo = 0; timeo < 1000; timeo++){
if(!(csr16r(ctlr, Cr) & Txon))
break;
microdelay(1);
}
ds->status = Own;
csr32w(ctlr, Txdaddr, PCIWADDR(ds));
}
if(ds->status & Own)
break;
ds->addr = 0;
ds->branch = 0;
if(ds->bp != nil){
freeb(ds->bp);
ds->bp = nil;
}
for(i = 0; i < Ntxstats-1; i++){
if(ds->status & (1<<i))
ctlr->txstats[i]++;
}
ctlr->txstats[i] += (ds->status & NcrMASK)>>NcrSHIFT;
ds = ds->next;
}
ctlr->tdh = ds;
/*
* Try to fill the ring back up.
*/
ds = ctlr->tdt;
while(tdused < ctlr->ntd-2){
if((bp = qget(edev->oq)) == nil)
break;
tdused++;
size = BLEN(bp);
prefix = 0;
if(o = (((int)bp->rp) & 0x03)){
prefix = Txcopy-o;
if(prefix > size)
prefix = size;
memmove(ds->bounce, bp->rp, prefix);
ds->addr = PCIWADDR(ds->bounce);
bp->rp += prefix;
size -= prefix;
}
next = ds->next;
ds->branch = PCIWADDR(ds->next);
if(size){
if(prefix){
next->bp = bp;
next->addr = PCIWADDR(bp->rp);
next->branch = PCIWADDR(next->next);
next->control = Edp|Chain|((size<<TbsSHIFT) & TbsMASK);
control = Stp|Chain|((prefix<<TbsSHIFT) & TbsMASK);
next = next->next;
tdused++;
ctlr->tsplit++;
}
else{
ds->bp = bp;
ds->addr = PCIWADDR(bp->rp);
control = Edp|Stp|((size<<TbsSHIFT) & TbsMASK);
ctlr->taligned++;
}
}
else{
freeb(bp);
control = Edp|Stp|((prefix<<TbsSHIFT) & TbsMASK);
ctlr->tcopied++;
}
ds->control = control;
if(tdused >= ctlr->ntd-2){
ds->control |= Ic;
ctlr->txdw++;
}
coherence();
ds->status = Own;
ds = next;
}
ctlr->tdt = ds;
ctlr->tdused = tdused;
if(ctlr->tdused)
csr16w(ctlr, Cr, Tdmd|ctlr->cr);
iunlock(&ctlr->tlock);
}
void
rudpkick(void *x)
{
Proc *up = externup();
Conv *c = x;
Udphdr *uh;
uint16_t rport;
uint8_t laddr[IPaddrlen], raddr[IPaddrlen];
Block *bp;
Rudpcb *ucb;
Rudphdr *rh;
Reliable *r;
int dlen, ptcllen;
Rudppriv *upriv;
Fs *f;
upriv = c->p->priv;
f = c->p->f;
netlog(c->p->f, Logrudp, "rudp: kick\n");
bp = qget(c->wq);
if(bp == nil)
return;
ucb = (Rudpcb*)c->ptcl;
switch(ucb->headers) {
case 7:
/* get user specified addresses */
bp = pullupblock(bp, UDP_USEAD7);
if(bp == nil)
return;
ipmove(raddr, bp->rp);
bp->rp += IPaddrlen;
ipmove(laddr, bp->rp);
bp->rp += IPaddrlen;
/* pick interface closest to dest */
if(ipforme(f, laddr) != Runi)
findlocalip(f, laddr, raddr);
bp->rp += IPaddrlen; /* Ignore ifc address */
rport = nhgets(bp->rp);
bp->rp += 2+2; /* Ignore local port */
break;
default:
ipmove(raddr, c->raddr);
ipmove(laddr, c->laddr);
rport = c->rport;
break;
}
dlen = blocklen(bp);
/* Make space to fit rudp & ip header */
bp = padblock(bp, UDP_IPHDR+UDP_RHDRSIZE);
if(bp == nil)
return;
uh = (Udphdr *)(bp->rp);
uh->vihl = IP_VER4;
rh = (Rudphdr*)uh;
ptcllen = dlen + (UDP_RHDRSIZE-UDP_PHDRSIZE);
uh->Unused = 0;
uh->udpproto = IP_UDPPROTO;
uh->frag[0] = 0;
uh->frag[1] = 0;
hnputs(uh->udpplen, ptcllen);
switch(ucb->headers){
case 7:
v6tov4(uh->udpdst, raddr);
hnputs(uh->udpdport, rport);
v6tov4(uh->udpsrc, laddr);
break;
default:
v6tov4(uh->udpdst, c->raddr);
hnputs(uh->udpdport, c->rport);
if(ipcmp(c->laddr, IPnoaddr) == 0)
findlocalip(f, c->laddr, c->raddr);
v6tov4(uh->udpsrc, c->laddr);
break;
}
hnputs(uh->udpsport, c->lport);
hnputs(uh->udplen, ptcllen);
uh->udpcksum[0] = 0;
uh->udpcksum[1] = 0;
qlock(&ucb->ql);
r = relstate(ucb, raddr, rport, "kick");
r->sndseq = NEXTSEQ(r->sndseq);
hnputl(rh->relseq, r->sndseq);
hnputl(rh->relsgen, r->sndgen);
hnputl(rh->relack, r->rcvseq); /* ACK last rcvd packet */
hnputl(rh->relagen, r->rcvgen);
if(r->rcvseq != r->acksent)
r->acksent = r->rcvseq;
hnputs(uh->udpcksum, ptclcsum(bp, UDP_IPHDR, dlen+UDP_RHDRSIZE));
relackq(r, bp);
qunlock(&ucb->ql);
upriv->ustats.rudpOutDatagrams++;
DPRINT("sent: %lud/%lud, %lud/%lud\n",
r->sndseq, r->sndgen, r->rcvseq, r->rcvgen);
doipoput(c, f, bp, 0, c->ttl, c->tos);
if(waserror()) {
relput(r);
qunlock(&r->lock);
nexterror();
}
/* flow control of sorts */
qlock(&r->lock);
if(UNACKED(r) > Maxunacked){
r->blocked = 1;
sleep(&r->vous, flow, r);
r->blocked = 0;
}
qunlock(&r->lock);
relput(r);
poperror();
}
int main(void) {
/* Create two empty queues */
void *q1;
void *q2;
void *pt;
int a,b,c,d,e,f,g,h,i,j,k;
int x;
int ii;
a = 1;
b = 4;
c = 65;
d = 34;
f = 6;
e = 9;
g = 11;
h = 23;
i = 999;
j = 234;
k = 94;
q1 = qopen();
q2 = qopen();
/* Fill both */
qput(q1, &a);
qput(q1, &b);
qput(q1, &c);
qput(q1, &d);
qput(q1, &e);
qput(q1, &f);
qput(q2, &g);
qput(q2, &h);
qput(q2, &i);
qput(q2, &j);
qput(q2, &k);
/* Get an item out of q1 */
pt = qget(q1);
if (*((int *)pt) != 1) {
printf("Fail on qget; returned %d.\n", *((int *)pt));
return -1;
}
/* Catenate */
qconcat(q1, q2);
/* Remove until null is returned, count number of iterations */
for (ii = 0; pt != NULL; ii++) {
pt = qget(q1);
}
if (ii != 11) {
printf("Fail on catenate; queue was of size %d.\n", ii);
return -1;
}
/* Use remove to take out a specified value */
qput(q1, &a);
qput(q1, &b);
qput(q1, &c);
qput(q1, &d);
qput(q1, &e);
qput(q1, &f);
qput(q1, &g);
qput(q1, &h);
qput(q1, &i);
qput(q1, &j);
qput(q1, &k);
x = 999;
pt = qremove(q1, searchfn, &x);
if (pt == NULL || *((int *)pt) != 999) {
if (pt == NULL) {
printf("Fail on qsearch; value not found.\n");
}
else {
printf("Fail on qremove / qsearch; removed value was %d.\n", *((int *)pt));
}
}
/* Use qapply to print out remaining list */
qapply(q1, printfn);
return 1;
}
