static int packet_callback(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
struct nfq_data *nfa, void *data)
{
QHandler *handler = (QHandler *) data;
u_int32_t id = get_pkt_id(nfa);
char *payload;
int payload_len;
payload_len = nfq_get_payload(nfa, &payload);
// Determine addresses
IpAddress srcaddr((struct in_addr *) (payload + 12));
IpAddress dstaddr((struct in_addr *) (payload + 16));
// std::cout << "Got packet ID " << id <<
// " from " << srcaddr << " to " << dstaddr << std::endl;
// Work out what proto it is...
int proto = (int) (unsigned char) payload[9];
FnvHash hash;
hash.addData(payload + 12, 8); // src + dst addr
if (proto == IPPROTO_TCP) {
// unsigned short sport = ntohs( *(unsigned short *) (payload+20));
// unsigned short dport = ntohs( *(unsigned short *) (payload+22));
// std::cout << "TCP sport " << sport << " dport " << dport
// << std::endl;
hash.addData(payload + 20, 4); // src + dst ports
}
int hashvalue = hash.get16rev();
// std::cout << "Hash value=" << hashvalue << std::endl;
u_int32_t verdict = NF_DROP;
if ((hashvalue >= handler->lowerHashLimit) && (hashvalue < handler->upperHashLimit))
verdict = NF_ACCEPT;
std::cout << "Connection from " << srcaddr << " Hash value=" << hashvalue << " Accept: " << (verdict == NF_ACCEPT ? "yes" : "no") << std::endl;
return nfq_set_verdict(handler->GetQh(), id, verdict, 0, NULL);
}
void
wifiiq(Wifi *wifi, Block *b)
{
SNAP s;
Wifipkt h, *w;
Etherpkt *e;
int hdrlen;
if(BLEN(b) < WIFIHDRSIZE)
goto drop;
w = (Wifipkt*)b->rp;
hdrlen = wifihdrlen(w);
if(BLEN(b) < hdrlen)
goto drop;
if(w->fc[1] & 0x40) {
/* encrypted */
qpass(wifi->iq, b);
return;
}
switch(w->fc[0] & 0x0c) {
case 0x00: /* management */
if((w->fc[1] & 3) != 0x00) /* STA->STA */
break;
qpass(wifi->iq, b);
return;
case 0x04: /* control */
break;
case 0x08: /* data */
b->rp += hdrlen;
switch(w->fc[0] & 0xf0) {
default:
goto drop;
case 0x80: /* QOS */
case 0x00:
break;
}
if(BLEN(b) < SNAPHDRSIZE)
break;
memmove(&s, b->rp, SNAPHDRSIZE);
if(s.dsap != 0xAA || s.ssap != 0xAA || s.control != 3)
break;
if(s.orgcode[0] != 0 || s.orgcode[1] != 0 || s.orgcode[2] != 0)
break;
b->rp += SNAPHDRSIZE-ETHERHDRSIZE;
h = *w;
e = (Etherpkt*)b->rp;
memmove(e->d, dstaddr(&h), Eaddrlen);
memmove(e->s, srcaddr(&h), Eaddrlen);
memmove(e->type, s.type, 2);
etheriq(wifi->ether, b, 1);
return;
}
drop:
freeb(b);
}
