void XBlockHead::finalize()
{
pullup(symMap);
std::vector<Block *>::iterator first;
std::vector<Block *>::iterator end = blocks.end();
for (first = blocks.begin(); first != end; ++first)
{
if (!(*first)->getReturn())
{
break;
}
}
if (first != end)
{
for (std::vector<Block *>::iterator i = std::next(first); i != end; ++i)
{
if (!(*i)->getReturn())
{
merge((*first)->getMap(), (*i)->getMap());
}
}
pullup((*first)->getMap());
}
}
static int ethertap_send(struct mbuf **bpp, struct iface *ifp, int32 gateway, uint8 tos)
{
int l;
struct edv_t *edv;
struct ethertap_packet ethertap_packet;
static const unsigned char ethernet_header[16] = {
0x00, 0x00, /* ??? */
0xfe, 0xfd, 0x00, 0x00, 0x00, 0x00, /* Destination address (kernel ethertap module) */
0xfe, 0xfe, 0x00, 0x00, 0x00, 0x00, /* Source address (WAMPES ethertap module) */
0x08, 0x00 /* Protocol (IP) */
};
edv = (struct edv_t *) ifp->edv;
dump(ifp, IF_TRACE_OUT, *bpp);
ifp->rawsndcnt++;
ifp->lastsent = secclock();
if (ifp->trace & IF_TRACE_RAW) {
raw_dump(ifp, -1, *bpp);
}
l = pullup(bpp, ethertap_packet.data, sizeof(ethertap_packet.data));
if (l <= 0 || *bpp) {
free_p(bpp);
return -1;
}
memcpy(ethertap_packet.ethernet_header, (const char *) ethernet_header, sizeof(ethernet_header));
write(edv->fd, ðertap_packet, l + sizeof(ethertap_packet.ethernet_header));
return l;
}
void MCP23009::init() {
// the same as Power-On/Reset value
IOMode(0xff); // set IODIR, 0x00 = all output
polarity(0x00);
config(0x00); // set IOCON w/ non-increment register address mode
pullup(0x00); // set no pull-ups
}
void MCP::init(MQTT &mqtt) {
DEBUG1_PRINTLN("MCP.init");
delay(50);
Wire.pins(4, 5);
MCP23017::begin(0);
pinMode(appSettings.m_int, INPUT);
this->mqtt = &mqtt;
for (byte i = 0; i < appSettings.msw_cnt; i++) {
//DEBUG4_PRINTLN("MCP.2");
MCP23017::pinMode(appSettings.msw[i], OUTPUT);
MCP23017::digitalWrite(appSettings.msw[i], actStates.msw[i]);
}
pullup(appSettings.m_int);
MCP23017::setupInterrupts(false, false, LOW);
for (byte i = 0; i < appSettings.min_cnt; i++) {
MCP23017::pinMode(appSettings.min[i], INPUT);
MCP23017::pullUp(appSettings.min[i], HIGH);
MCP23017::setupInterruptPin(appSettings.min[i], FALLING);
}
attachInterrupt(appSettings.m_int, Delegate<void()>(&MCP::interruptCallback, this), FALLING);
//timer.initializeMs(30 * 1000, TimerDelegate(&MCP::publish, this)).start(); // every 25 seconds
interruptReset();
}
/* Decrement the IP TTL field in each packet on the send queue. If
* a TTL goes to zero, discard the packet.
*/
void
ttldec(
struct iface *ifp
){
struct mbuf *bp,*bpprev,*bpnext;
struct qhdr qhdr;
struct ip ip;
bpprev = NULL;
for(bp = ifp->outq; bp != NULL;bpprev = bp,bp = bpnext){
bpnext = bp->anext;
pullup(&bp,&qhdr,sizeof(qhdr));
ntohip(&ip,&bp);
if(--ip.ttl == 0){
/* Drop packet */
icmp_output(&ip,bp,ICMP_TIME_EXCEED,0,NULL);
if(bpprev == NULL) /* First on queue */
ifp->outq = bpnext;
else
bpprev->anext = bpnext;
free_p(&bp);
bp = bpprev;
continue;
}
/* Put IP and queue headers back, restore to queue */
htonip(&ip,&bp,0);
pushdown(&bp,&qhdr,sizeof(qhdr));
if(bpprev == NULL) /* First on queue */
ifp->outq = bp;
else
bpprev->anext = bp;
bp->anext = bpnext;
}
}
/*
|| <<constructor>>
|| @parameter buttonPin sets the pin that this switch is connected to
|| @parameter buttonMode indicates PULLUP or PULLDOWN resistor
*/
Button::Button(uint8_t buttonPin, uint8_t buttonMode){
this->pin=buttonPin;
pinMode(pin,INPUT);
buttonMode==PULLDOWN ? pulldown() : pullup();
state = 0;
bitWrite(state,CURRENT,!mode);
}
static int ipip_send(struct mbuf **bpp, struct iface *ifp, int32 gateway, uint8 tos)
{
char buf[MAX_FRAME];
int l;
struct edv_t *edv;
struct sockaddr_in addr;
dump(ifp, IF_TRACE_OUT, *bpp);
ifp->rawsndcnt++;
ifp->lastsent = secclock();
if (ifp->trace & IF_TRACE_RAW)
raw_dump(ifp, -1, *bpp);
l = pullup(bpp, buf, sizeof(buf));
if (l <= 0 || *bpp) {
free_p(bpp);
return -1;
}
edv = (struct edv_t *) ifp->edv;
addr.sin_family = AF_INET;
addr.sin_addr.s_addr = htonl(gateway);
addr.sin_port = htons(edv->port);
sendto(edv->fd, buf, l, 0, (struct sockaddr *) &addr, sizeof(addr));
return l;
}
void CButton::begin()
{
Module::begin();
pinMode(pin,INPUT);
mode==PULLDOWN ? pulldown() : pullup();
state = 0;
timing[CHANGED] = timing[PREVIOUS] = timing[CURRENT] = 0;
bitWrite(state,CURRENT,!mode);
stopCountingSequence=shortCounted=longCounted=false;
}
/* Convert a network-format AX.25 header into a host format structure
* Return -1 if error, number of addresses if OK
*/
int
ntohax25(
register struct ax25 *hdr, /* Output structure */
struct mbuf **bpp
){
register uint8 *axp;
if(pullup(bpp,hdr->dest,AXALEN) < AXALEN)
return -1;
if(pullup(bpp,hdr->source,AXALEN) < AXALEN)
return -1;
/* Process C bits to get command/response indication */
if((hdr->source[ALEN] & C) == (hdr->dest[ALEN] & C))
hdr->cmdrsp = LAPB_UNKNOWN;
else if(hdr->source[ALEN] & C)
hdr->cmdrsp = LAPB_RESPONSE;
else
hdr->cmdrsp = LAPB_COMMAND;
hdr->ndigis = 0;
hdr->nextdigi = 0;
if(hdr->source[ALEN] & E)
return 2; /* No digis */
/* Count and process the digipeaters */
axp = hdr->digis[0];
while(hdr->ndigis < MAXDIGIS && pullup(bpp,axp,AXALEN) == AXALEN){
hdr->ndigis++;
if(axp[ALEN] & REPEATED)
hdr->nextdigi++;
if(axp[ALEN] & E) /* Last one */
return hdr->ndigis + 2;
axp += AXALEN;
}
return -1; /* Too many digis */
}
/*
|| @constructor
|| | Set the initial state of this button
|| #
||
|| @parameter buttonPin sets the pin that this switch is connected to
|| @parameter buttonMode indicates BUTTON_PULLUP or BUTTON_PULLDOWN resistor
*/
Button::Button(uint8_t buttonPin, uint8_t buttonMode){
pin=buttonPin;
pinMode(pin,INPUT);
buttonMode==BUTTON_PULLDOWN ? pulldown() : pullup(buttonMode);
state = 0;
bitWrite(state,CURRENT,!mode);
cb_onPress = 0;
cb_onRelease = 0;
cb_onClick = 0;
cb_onHold = 0;
numberOfPresses = 0;
triggeredHoldEvent = true;
}
static void tcp_receive(struct tcb *tcb, int32 cnt)
{
char buffer[1024];
struct mbuf *bp;
if (tcb->user > 0) {
recv_tcp(tcb, &bp, 0);
while ((cnt = pullup(&bp, buffer, sizeof(buffer))) > 0)
if (write(tcb->user, buffer, (unsigned) cnt) != cnt) {
free_p(&bp);
close_tcp(tcb);
return;
}
}
}
/* Dump an mbuf in hex */
void
hex_dump(
FILE *fp,
struct mbuf **bpp)
{
uint n;
uint address;
uint8 buf[16];
if(bpp == NULL || *bpp == NULL || fp == NULL)
return;
address = 0;
while((n = pullup(bpp,buf,sizeof(buf))) != 0){
fmtline(fp,address,buf,n);
address += n;
}
}
/* Check remote password */
static int
chkrpass(
struct mbuf *bp)
{
char *lbuf;
uint16 len;
int rval = 0;
len = len_p(bp);
if(Rempass == 0 || *Rempass == 0 || strlen(Rempass) != len)
return rval;
lbuf = (char *) mallocw(len);
pullup(&bp,lbuf,len);
if(strncmp(Rempass,lbuf,len) == 0)
rval = 1;
free(lbuf);
return rval;
}
/*
|| @constructor
|| | Set the initial state of this button
|| #
||
|| @parameter buttonPin sets the pin that this switch is connected to
|| @parameter buttonMode indicates BUTTON_PULLUP or BUTTON_PULLDOWN resistor
*/
Button::Button(uint8_t buttonPin, uint8_t buttonMode, bool _debounceMode, int _debounceDuration){
pin=buttonPin;
pinMode(pin,INPUT);
debounceMode = _debounceMode;
debounceDuration = _debounceDuration;
debounceStartTime = millis();
buttonMode==BUTTON_PULLDOWN ? pulldown() : pullup(buttonMode);
state = 0;
bitWrite(state,CURRENT,!mode);
cb_onPress = 0;
cb_onRelease = 0;
cb_onClick = 0;
cb_onHold = 0;
numberOfPresses = 0;
triggeredHoldEvent = true;
}
int def(tokens_t *tp, tokens_t **ep)
{
tokens_t *t;
preproc_t *p;
strs_t *st;
if (tp->ttype == TT_OPR && tp->num == OPR_SIZEOF) {
tp++;
syntax(tp->ttype == TT_SYM, "expected sizeof sym");
if ((p = pre(tp->str, PT_STRUCT))) {
if (debug) printf("sizeof %s <- %d\n", tp->str, p->size);
pullup(tp, tp+1, ep);
tp--;
tp->ttype = TT_NUM;
tp->num = p->size;
tp++;
return 1;
}
if (debug) printf("unknown sizeof: %s (likely fwd ref)\n", tp->str);
}
if (tp->ttype == TT_SYM) {
if ((p = pre(tp->str, PT_DEF))) {
//if (debug) printf("def: preproc %s %s\n", p->str, tp->str);
if (debug) printf("DEF \"%s\" <- %d\n", tp->str, p->val);
tp->ttype = TT_NUM;
tp->num = p->val;
return 1;
}
if ((st = string_find(tp->str)) && (st->flags & SF_DEFINED)) {
if (debug) printf("SYM is now a defined LABEL \"%s\" <- 0x%x\n", tp->str, st->val);
tp->ttype = TT_NUM;
tp->num = st->val;
return 1;
}
}
return 0;
}
/* Execute random quench algorithm on an interface's output queue */
void
rquench(
struct iface *ifp,
int drop
){
struct mbuf *bp,*bplast;
int i;
struct qhdr qhdr;
struct ip ip;
struct mbuf *bpdup;
if((i = len_q(ifp->outq)) == 0)
return; /* Queue is empty */
i = urandom(i); /* Select a victim */
/* Search for i-th message on queue */
bplast = NULL;
for(bp = ifp->outq;bp != NULL && i>0;i--,bplast=bp,bp=bp->anext)
;
if(bp == NULL)
return; /* "Can't happen" */
/* Send a source quench */
dup_p(&bpdup,bp,0,len_p(bp));
pullup(&bpdup,&qhdr,sizeof(qhdr));
ntohip(&ip,&bpdup);
icmp_output(&ip,bpdup,ICMP_QUENCH,0,NULL);
free_p(&bpdup);
if(!drop)
return; /* All done */
/* Drop the packet */
if(bplast != NULL)
bplast->anext = bp->anext;
else
ifp->outq = bp->anext; /* First on list */
free_p(&bp);
}
bool DHT::read(void)
{
uint8_t laststate = HIGH;
uint8_t counter = 0;
uint8_t j = 0, i;
unsigned long time = millis();
//Determine if it's appropiate to read the sensor, or return data from cache
if ((time - m_lastreadtime) < m_maxIntervalRead && (errDHT_OK == m_lastError))
{
return true; // will use last data from cache
}
m_lastreadtime = time;
//reset internal data and invalidate cache
m_data[0] = m_data[1] = m_data[2] = m_data[3] = m_data[4] = 0;
m_lastError = errDHT_Other;
m_lastTemp = NAN;
m_lastHumid = NAN;
//Pull the pin low for m_wakeupTimeMs milliseconds
pinMode(m_kSensorPin, OUTPUT);
digitalWrite(m_kSensorPin, LOW);
delay(m_wakeupTimeMs);
//clear interrupts
cli();
//Make pin input and activate pullup
pinMode(m_kSensorPin, INPUT);
if (m_bPullupEnabled)
{
pullup(m_kSensorPin);
}
else
{
digitalWrite(m_kSensorPin, HIGH);
}
//Read in the transitions
for (i = 0; i < MAXTIMINGS || j >= 40; i++)
{
counter = 0;
while (digitalRead(m_kSensorPin) == laststate)
{
counter++;
delayMicroseconds(1);
if (counter == 255)
{
break;
}
}
laststate = digitalRead(m_kSensorPin);
if (counter == 255)
{
m_lastError = errDHT_Timeout;
break;
}
// ignore first 3 transitions
if ((i >= 4) && (i % 2 == 0))
{
// shove each bit into the storage bytes
m_data[j / 8] <<= 1;
if (counter > ONE_DURATION_THRESH_US)
{
m_data[j / 8] |= 1;
}
j++;
}
}
sei();
#if DHT_DEBUG
Serial.println(j, DEC);
Serial.print(m_data[0], HEX); Serial.print(", ");
Serial.print(m_data[1], HEX); Serial.print(", ");
Serial.print(m_data[2], HEX); Serial.print(", ");
Serial.print(m_data[3], HEX); Serial.print(", ");
Serial.print(m_data[4], HEX); Serial.print(" =? ");
Serial.println((m_data[0] + m_data[1] + m_data[2] + m_data[3]) & 0xFF, HEX);
#endif
// pull the pin high at the end
//(will stay high at least 250ms until the next reading)
pinMode(m_kSensorPin, OUTPUT);
digitalWrite(m_kSensorPin, HIGH);
// check we read 40 bits and that the checksum matches
if ((j >= 40) &&
(m_data[4] == ((m_data[0] + m_data[1] + m_data[2] + m_data[3]) & 0xFF)))
{
updateInternalCache();
m_lastError = errDHT_OK;
return true;
}
else
{
m_lastError = errDHT_Checksum;
}
return false;
}
/* Process IP datagram fragments
* If datagram is complete, return its length (MINUS header);
* otherwise return -1
*/
static int
fraghandle(
struct ip *ip, /* IP header, host byte order */
struct mbuf **bpp /* The fragment itself */
){
register struct reasm *rp; /* Pointer to reassembly descriptor */
struct frag *lastfrag,*nextfrag,*tfp;
struct mbuf *tbp;
uint16 i;
uint16 last; /* Index of first byte beyond fragment */
last = ip->offset + ip->length - (IPLEN + ip->optlen);
rp = lookup_reasm(ip);
if(ip->offset == 0 && !ip->flags.mf){
/* Complete datagram received. Discard any earlier fragments */
if(rp != NULL){
free_reasm(rp);
ipReasmOKs++;
}
return ip->length;
}
ipReasmReqds++;
if(rp == NULL){
/* First fragment; create new reassembly descriptor */
if((rp = creat_reasm(ip)) == NULL){
/* No space for descriptor, drop fragment */
ipReasmFails++;
free_p(bpp);
return -1;
}
}
/* Keep restarting timer as long as we keep getting fragments */
stop_timer(&rp->timer);
start_timer(&rp->timer);
/* If this is the last fragment, we now know how long the
* entire datagram is; record it
*/
if(!ip->flags.mf)
rp->length = last;
/* Set nextfrag to the first fragment which begins after us,
* and lastfrag to the last fragment which begins before us
*/
lastfrag = NULL;
for(nextfrag = rp->fraglist;nextfrag != NULL;nextfrag = nextfrag->next){
if(nextfrag->offset > ip->offset)
break;
lastfrag = nextfrag;
}
/* Check for overlap with preceeding fragment */
if(lastfrag != NULL && ip->offset < lastfrag->last){
/* Strip overlap from new fragment */
i = lastfrag->last - ip->offset;
pullup(bpp,NULL,i);
if(*bpp == NULL)
return -1; /* Nothing left */
ip->offset += i;
}
/* Look for overlap with succeeding segments */
for(; nextfrag != NULL; nextfrag = tfp){
tfp = nextfrag->next; /* save in case we delete fp */
if(nextfrag->offset >= last)
break; /* Past our end */
/* Trim the front of this entry; if nothing is
* left, remove it.
*/
i = last - nextfrag->offset;
pullup(&nextfrag->buf,NULL,i);
if(nextfrag->buf == NULL){
/* superseded; delete from list */
if(nextfrag->prev != NULL)
nextfrag->prev->next = nextfrag->next;
else
rp->fraglist = nextfrag->next;
if(tfp->next != NULL)
nextfrag->next->prev = nextfrag->prev;
freefrag(nextfrag);
} else
nextfrag->offset = last;
}
/* Lastfrag now points, as before, to the fragment before us;
* nextfrag points at the next fragment. Check to see if we can
* join to either or both fragments.
*/
i = INSERT;
if(lastfrag != NULL && lastfrag->last == ip->offset)
i |= APPEND;
if(nextfrag != NULL && nextfrag->offset == last)
i |= PREPEND;
switch(i){
case INSERT: /* Insert new desc between lastfrag and nextfrag */
tfp = newfrag(ip->offset,last,bpp);
tfp->prev = lastfrag;
tfp->next = nextfrag;
if(lastfrag != NULL)
lastfrag->next = tfp; /* Middle of list */
else
rp->fraglist = tfp; /* First on list */
if(nextfrag != NULL)
nextfrag->prev = tfp;
break;
case APPEND: /* Append to lastfrag */
append(&lastfrag->buf,bpp);
lastfrag->last = last; /* Extend forward */
break;
case PREPEND: /* Prepend to nextfrag */
tbp = nextfrag->buf;
nextfrag->buf = *bpp;
bpp = NULL;
append(&nextfrag->buf,&tbp);
nextfrag->offset = ip->offset; /* Extend backward */
break;
case (APPEND|PREPEND):
/* Consolidate by appending this fragment and nextfrag
* to lastfrag and removing the nextfrag descriptor
*/
append(&lastfrag->buf,bpp);
append(&lastfrag->buf,&nextfrag->buf);
nextfrag->buf = NULL;
lastfrag->last = nextfrag->last;
/* Finally unlink and delete the now unneeded nextfrag */
lastfrag->next = nextfrag->next;
if(nextfrag->next != NULL)
nextfrag->next->prev = lastfrag;
freefrag(nextfrag);
break;
}
if(rp->fraglist->offset == 0 && rp->fraglist->next == NULL
&& rp->length != 0){
/* We've gotten a complete datagram, so extract it from the
* reassembly buffer and pass it on.
*/
*bpp = rp->fraglist->buf;
rp->fraglist->buf = NULL;
/* Tell IP the entire length */
ip->length = rp->length + (IPLEN + ip->optlen);
free_reasm(rp);
ipReasmOKs++;
ip->offset = 0;
ip->flags.mf = 0;
return ip->length;
} else
return -1;
}
void Block::finalize()
{
pullup(symMap);
}
int main(void) {
// set clock divider to /1
CLKPR = (1 << CLKPCE);
CLKPR = (0 << CLKPS3) | (0 << CLKPS2) | (0 << CLKPS1) | (0 << CLKPS0);
// initialize rgb output pins
set(led_r_port, led_r_pin);
set(led_g_port, led_g_pin);
set(led_b_port, led_b_pin);
output(led_r_direction, led_r_pin);
output(led_g_direction, led_g_pin);
output(led_b_direction, led_b_pin);
input(buttons_direction, button_0_pin);
input(buttons_direction, button_1_pin);
input(buttons_direction, button_2_pin);
input(buttons_direction, button_3_pin);
pullup(buttons_port, button_0_pin);
pullup(buttons_port, button_1_pin);
pullup(buttons_port, button_2_pin);
pullup(buttons_port, button_3_pin);
unsigned int elapsed_us = 0;
int button_polls_per_second = 10;
int button_states[4] = {0, 0, 0, 0};
int rgb_pulses_per_ms[3] = {0, 0, 0};
while (1) {
_delay_us(1);
elapsed_us++;
// poll button state
if (elapsed_us % 1000 < button_polls_per_second) {
if (!pin_test(buttons_pins, button_0_pin)) {
button_states[0] = 10;
} else if (button_states[0]) {
button_states[0]--;
}
if (!pin_test(buttons_pins, button_1_pin)) {
button_states[1] = 10;
} else if (button_states[1]) {
button_states[1]--;
}
if (!pin_test(buttons_pins, button_2_pin)) {
button_states[2] = 10;
} else if (button_states[2]) {
button_states[2]--;
}
if (!pin_test(buttons_pins, button_3_pin)) {
button_states[3] = 10;
} else if (button_states[3]) {
button_states[3]--;
}
}
// calculate rgb values
set_rgb_pulse_rates(button_states, rgb_pulses_per_ms);
// update red led
if (elapsed_us % 1000 < rgb_pulses_per_ms[0]) {
clear(led_r_port, led_r_pin);
} else {
set(led_r_port, led_r_pin);
}
// update green led
if (elapsed_us % 1000 < rgb_pulses_per_ms[1]) {
clear(led_g_port, led_g_pin);
} else {
set(led_g_port, led_g_pin);
}
// update blue led
if (elapsed_us % 1000 < rgb_pulses_per_ms[2]) {
clear(led_b_port, led_b_pin);
} else {
set(led_b_port, led_b_pin);
}
}
}
/* Process an incoming ICMP packet */
void
icmp_input(
struct iface *iface, /* Incoming interface (ignored) */
struct ip *ip, /* Pointer to decoded IP header structure */
struct mbuf **bpp, /* Pointer to ICMP message */
int rxbroadcast,
int32 said
){
struct icmplink *ipp;
struct icmp icmp; /* ICMP header */
struct ip oip; /* Offending datagram header */
uint type; /* Type of ICMP message */
uint length;
icmpInMsgs++;
if(rxbroadcast){
/* Broadcast ICMP packets are to be IGNORED !! */
icmpInErrors++;
free_p(bpp);
return;
}
length = ip->length - IPLEN - ip->optlen;
if(cksum(NULL,*bpp,length) != 0){
/* Bad ICMP checksum; discard */
icmpInErrors++;
free_p(bpp);
return;
}
ntohicmp(&icmp,bpp);
/* Process the message. Some messages are passed up to the protocol
* module for handling, others are handled here.
*/
type = icmp.type;
switch(type){
case ICMP_TIME_EXCEED: /* Time-to-live Exceeded */
case ICMP_DEST_UNREACH: /* Destination Unreachable */
case ICMP_QUENCH: /* Source Quench */
case ICMP_IPSP: /* Bad security packet */
switch(type){
case ICMP_TIME_EXCEED: /* Time-to-live Exceeded */
icmpInTimeExcds++;
break;
case ICMP_DEST_UNREACH: /* Destination Unreachable */
icmpInDestUnreachs++;
break;
case ICMP_QUENCH: /* Source Quench */
icmpInSrcQuenchs++;
break;
}
ntohip(&oip,bpp); /* Extract offending IP header */
if(Icmp_trace){
printf("ICMP from %s:",inet_ntoa(ip->source));
printf(" dest %s %s",inet_ntoa(oip.dest),
smsg(Icmptypes,ICMP_TYPES,type));
switch(type){
case ICMP_TIME_EXCEED:
printf(" %s\n",
smsg(Exceed,NEXCEED,icmp.code));
break;
case ICMP_DEST_UNREACH:
printf(" %s\n",
smsg(Unreach,NUNREACH,icmp.code));
break;
case ICMP_IPSP:
printf(" %s\n",smsg(Said_icmp,NIPSP,icmp.code));
break;
default:
printf(" %u\n",icmp.code);
break;
}
}
for(ipp = Icmplink;ipp->funct != NULL;ipp++)
if(ipp->proto == oip.protocol)
break;
if(ipp->funct != NULL){
(*ipp->funct)(ip->source,oip.source,oip.dest,icmp.type,
icmp.code,bpp);
}
break;
case ICMP_ECHO: /* Echo Request */
/* Change type to ECHO_REPLY, recompute checksum,
* and return datagram.
*/
icmpInEchos++;
icmp.type = ICMP_ECHO_REPLY;
htonicmp(&icmp,bpp);
icmpOutEchoReps++;
{
int32 tmp = ip->source;
ip->source = ip->dest;
ip->dest = tmp;
ip->ttl = (char) ipDefaultTTL;
htonip(ip,bpp,IP_CS_NEW);
icmpOutMsgs++;
net_route(NULL,bpp);
}
return;
case ICMP_REDIRECT: /* Redirect */
icmpInRedirects++;
ntohip(&oip,bpp); /* Extract offending IP header */
if(Icmp_trace){
printf("ICMP from %s:",inet_ntoa(ip->source));
printf(" dest %s %s",inet_ntoa(oip.dest),
smsg(Icmptypes,ICMP_TYPES,type));
printf(" new gateway %s\n",inet_ntoa(icmp.args.address));
}
break;
case ICMP_PARAM_PROB: /* Parameter Problem */
icmpInParmProbs++;
break;
case ICMP_ECHO_REPLY: /* Echo Reply */
icmpInEchoReps++;
echo_proc(ip->source,ip->dest,&icmp,bpp);
break;
case ICMP_TIMESTAMP: /* Timestamp */
icmpInTimestamps++;
{
int32 tmp;
uint8 buf[12];
struct timeval tv;
if(pullup(bpp,buf,sizeof(buf)) != sizeof(buf)){
free_p(bpp);
return;
}
gettimeofday(&tv,0);
tmp = (tv.tv_sec % 86400) * 1000 + tv.tv_usec / 1000;
put32(&buf[4],tmp); /* Receive Timestamp */
put32(&buf[8],tmp); /* Transmit Timestamp */
pushdown(bpp,buf,sizeof(buf));
icmp.type = ICMP_TIME_REPLY;
htonicmp(&icmp,bpp);
icmpOutTimestampReps++;
tmp = ip->source;
ip->source = ip->dest;
ip->dest = tmp;
ip->ttl = (char) ipDefaultTTL;
htonip(ip,bpp,IP_CS_NEW);
icmpOutMsgs++;
net_route(NULL,bpp);
return;
}
case ICMP_TIME_REPLY: /* Timestamp Reply */
icmpInTimestampReps++;
break;
case ICMP_INFO_RQST: /* Information Request */
break;
case ICMP_INFO_REPLY: /* Information Reply */
break;
}
free_p(bpp);
}
/* Pull TCP header off mbuf */
int
ntohtcp(
struct tcp *tcph,
struct mbuf **bpp
){
int hdrlen,i,optlen,kind;
register int flags;
uint8 hdrbuf[TCPLEN],*cp;
uint8 options[TCP_MAXOPT];
memset(tcph,0,sizeof(struct tcp));
i = pullup(bpp,hdrbuf,TCPLEN);
/* Note that the results will be garbage if the header is too short.
* We don't check for this because returned ICMP messages will be
* truncated, and we at least want to get the port numbers.
*/
tcph->source = get16(&hdrbuf[0]);
tcph->dest = get16(&hdrbuf[2]);
tcph->seq = get32(&hdrbuf[4]);
tcph->ack = get32(&hdrbuf[8]);
hdrlen = (hdrbuf[12] & 0xf0) >> 2;
flags = hdrbuf[13];
tcph->flags.congest = (flags & 64) ? 1 : 0;
tcph->flags.urg = (flags & 32) ? 1 : 0;
tcph->flags.ack = (flags & 16) ? 1 : 0;
tcph->flags.psh = (flags & 8) ? 1 : 0;
tcph->flags.rst = (flags & 4) ? 1 : 0;
tcph->flags.syn = (flags & 2) ? 1 : 0;
tcph->flags.fin = (flags & 1) ? 1 : 0;
tcph->wnd = get16(&hdrbuf[14]);
tcph->checksum = get16(&hdrbuf[16]);
tcph->up = get16(&hdrbuf[18]);
optlen = hdrlen - TCPLEN;
/* Check for option field */
if(i < TCPLEN || hdrlen < TCPLEN)
return -1; /* Header smaller than legal minimum */
if(optlen == 0)
return (int)hdrlen; /* No options, all done */
if(optlen > len_p(*bpp)){
/* Remainder too short for options length specified */
return -1;
}
pullup(bpp,options,optlen); /* "Can't fail" */
/* Process options */
for(cp=options,i=optlen; i > 0;){
kind = *cp++;
i--;
/* Process single-byte options */
switch(kind){
case EOL_KIND:
return (int)hdrlen; /* End of options list */
case NOOP_KIND:
continue; /* Go look for next option */
}
/* All other options have a length field */
optlen = *cp++;
/* Process valid multi-byte options */
switch(kind){
case MSS_KIND:
if(optlen == MSS_LENGTH){
tcph->mss = get16(cp);
tcph->flags.mss = 1;
}
break;
case WSCALE_KIND:
if(optlen == WSCALE_LENGTH){
tcph->wsopt = *cp;
tcph->flags.wscale = 1;
}
break;
case TSTAMP_KIND:
if(optlen == TSTAMP_LENGTH){
tcph->tsval = get32(cp);
tcph->tsecr = get32(cp+4);
tcph->flags.tstamp = 1;
}
break;
}
optlen = max(2,optlen); /* Enforce legal minimum */
i -= optlen;
cp += optlen - 2;
}
return (int)hdrlen;
}
