static packet* EthDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
struct ethhdr *eth;
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
eth = (struct ethhdr *)pkt->data;
/* set attribute */
memcpy(val.mac, eth->h_source, FT_ETH_LEN);
ProtInsAttr(frame, mac_src_id, &val);
val.uint16 = ntohs(eth->h_proto);
ProtInsAttr(frame, etype_id, &val);
/* pdu */
pkt->data += hweth_len;
pkt->len -= hweth_len;
return pkt;
}
static packet* ChdlcDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
int len;
unsigned char prot;
int proto_offset;
unsigned short chdlc_prot;
len = 0;
proto_offset = 2;
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
chdlc_prot = ntohs(*((uint16_t *)(pkt->data + proto_offset)));
len = 2;
val.uint16 = chdlc_prot;
ProtInsAttr(frame, proto_id, &val);
/* pdu */
pkt->data += len + proto_offset;
pkt->len -= len + proto_offset;
return pkt;
}
static packet* PppoeDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
unsigned short len;
pppoe_hdr *pppoeh;
/* simple verify */
if (sizeof(pppoe_hdr) > pkt->len) {
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
len = 0;
pppoeh = (pppoe_hdr*)pkt->data;
len = ntohs(pppoeh->len);
if (len > pkt->len - sizeof(pppoe_hdr)) {
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
/* type */
val.uint8 = pppoeh->type;
ProtInsAttr(frame, type_id, &val);
/* code */
val.uint8 = pppoeh->code;
ProtInsAttr(frame, code_id, &val);
/* session id */
val.uint16 = ntohs(pppoeh->sess_id);
ProtInsAttr(frame, ses_id, &val);
/* pdu */
pkt->data += sizeof(pppoe_hdr);
pkt->len = len;
return pkt;
}
static packet* Ieee80211Dissector(packet *pkt)
{
pstack_f *frame;
ftval val;
int len;
struct ieee80211hdr *ie80211;
/* check minimal len (only data frame) */
if (pkt->len < sizeof(struct ieee80211hdr))
return NULL;
len = 0;
ie80211 = (struct ieee80211hdr *)pkt->data;
/* only data frame with data */
if (ie80211->u1.fc.type != DATA_FRAME ||
(ie80211->u1.fc.subtype & DATA_NULL_MASK) == DATA_NULL_MASK) {
return NULL;
}
/* Handle QoS */
if ((ie80211->u1.fc.subtype & DATA_QOS_MASK) == DATA_QOS_MASK) {
len += 2;
}
/* data addr */
if (ie80211->u1.fc.to_ds == 0 && ie80211->u1.fc.from_ds == 0) {
memcpy(val.mac, ie80211->addr3, FT_ETH_LEN);
len += DATA_SHORT_HDR_LEN;
} else if (ie80211->u1.fc.to_ds == 0 && ie80211->u1.fc.from_ds == 1) {
memcpy(val.mac, ie80211->addr2, FT_ETH_LEN);
len += DATA_SHORT_HDR_LEN;
} else if (ie80211->u1.fc.to_ds == 1 && ie80211->u1.fc.from_ds == 0) {
memcpy(val.mac, ie80211->addr1, FT_ETH_LEN);
len += DATA_SHORT_HDR_LEN;
} else if (ie80211->u1.fc.to_ds == 1 && ie80211->u1.fc.from_ds == 1) {
memcpy(val.mac, ie80211->addr2, FT_ETH_LEN);
len += DATA_LONG_HDR_LEN;
}
if (pkt->len < len)
return NULL;
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
ProtInsAttr(frame, bss_id, &val);
/* pdu */
pkt->data += len;
pkt->len -= len;
if (llc_id != -1)
return ProtDissecPkt(llc_id, pkt);
return pkt;
}
static packet* SllDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
struct sll_header *psll;
unsigned short addr_len;
/* check consistence */
if (pkt->len < sizeof(struct sll_header)) {
LogPrintf(LV_WARNING, "SLL packet dimension overflow the real dimension of packet");
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
psll = (struct sll_header *)pkt->data;
addr_len = ntohs(psll->sll_halen);
if (addr_len > SLL_ADDRLEN) {
LogPrintf(LV_WARNING, "SLL frame error");
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
#if 0
val.uint16 = ntohs(psll->sll_pkttype);
ProtInsAttr(frame, pkttype_id, &val);
val.uint16 = ntohs(psll->sll_hatype);
ProtInsAttr(frame, hatype_id, &val);
val.uint16 = addr_len;
ProtInsAttr(frame, halen_id, &val);
#endif
val.uint16 = ntohs(psll->sll_protocol);
ProtInsAttr(frame, protocol_id, &val);
/* pdu */
pkt->data += sizeof(struct sll_header);
pkt->len -= sizeof(struct sll_header);
return pkt;
}
static packet* VlanDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
int proto_offset;
unsigned short proto, vid;
unsigned short data;
/* header */
data = ntohs(*((uint16_t *)pkt->data));
proto_offset = 2;
/* vid */
vid = data & 0x0FFF;
/* protocol */
proto = ntohs(*(uint16_t *)(pkt->data + proto_offset));
proto_offset += 2;
if (proto <= IEEE_802_3_MAX_LEN) {
/* to be implemented */
LogPrintf(LV_DEBUG, "Unknow protocol:%i ", proto);
PktFree(pkt);
return NULL;
}
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
val.uint16 = vid;
ProtInsAttr(frame, vid_id, &val);
val.uint16 = proto;
ProtInsAttr(frame, proto_id, &val);
/* pdu */
pkt->data += proto_offset;
pkt->len -= proto_offset;
return pkt;
}
static packet* NullDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
unsigned int null_hdr;
unsigned short len;
/* check consistence */
if (pkt->len < 4) {
LogPrintf(LV_WARNING, "Null packet dimension overflow the real dimension of packet");
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
/* check if there is PPP */
if (ntohs(*((unsigned short *)pkt->data)) == 0xFF03) {
LogPrintf(LV_WARNING, "Null packet contents is PPP (to develop)");
return NULL;
}
len = sizeof(unsigned int);
memcpy(&null_hdr, pkt->data, len);
if ((null_hdr & 0xFFFF0000) != 0) {
/* swap bytes */
#warning "to complete"
return NULL;
}
if (null_hdr > IEEE_802_3_MAX_LEN) {
#warning "to complete"
return NULL;
}
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
val.uint32 = null_hdr;
ProtInsAttr(frame, family_id, &val);
/* pdu */
pkt->data += len;
pkt->len -= len;
return pkt;
}
static packet *GtpDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
gtphdr *gtp_h;
unsigned short offset;
unsigned char neht, nhlen;
if (pkt->len < GTP_MIN_HEADER_SIZE) {
LogPrintf(LV_WARNING, "GTP V1 size error");
PktFree(pkt);
return NULL;
}
/* header */
gtp_h = (gtphdr *)pkt->data;
/* gtp version */
if (gtp_h->ver != 1) {
LogPrintf(LV_WARNING, "GTP version error (ver:%i)", gtp_h->ver);
GtpPrintHdr(gtp_h);
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
offset = 8;
if (gtp_h->ext || gtp_h->seq || gtp_h->npdu) {
offset += 4;
if (gtp_h->ext) {
/* decode extension header */
neht = gtp_h->neht;
while (neht != 0) {
nhlen = pkt->data[offset];
offset += nhlen;
neht = pkt->data[offset-1];
}
}
//GtpPrintHdr(gtp_h);
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
val.uint32 = gtp_h->teid;
ProtInsAttr(frame, tunnel_id, &val);
val.uint8 = gtp_h->mtype;
ProtInsAttr(frame, proto_id, &val);
/* pdu */
pkt->data += offset;
pkt->len -= offset;
//LogPrintf(LV_DEBUG, "data: 0x%x 0x%x", pkt->data[0], pkt->data[1]);
return pkt;
}
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
#if 0
/* gtp version */
if (gtp_h->ver != 1) {
LogPrintf(LV_WARNING, "GTP version error (ver:%i)", gtp_h->ver);
GtpPrintHdr(gtp_h);
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
proto_offset += 2;
/* control message */
if (gtp_h->t == 1) {
LogPrintf(LV_DEBUG, "Control message gtp ver:%i ", gtp_h->ver);
PktFree(pkt);
return NULL;
}
/* length */
if (gtp_h->l == 1) {
length = ntohs(*(uint16_t *)(pkt->data + proto_offset));
//LogPrintf(LV_DEBUG, "Length message: %i", length);
proto_offset += 2;
}
/* tunnel and session id */
tunnel = ntohs(*(uint16_t *)(pkt->data + proto_offset));
proto_offset += 2;
session = ntohs(*(uint16_t *)(pkt->data + proto_offset));
proto_offset += 2;
/* Ns and Nr fields */
if (gtp_h->s == 1) {
LogPrintf(LV_DEBUG, "Ns and Nr fields");
ProtStackFrmDisp(pkt->stk, TRUE);
proto_offset += 4;
}
/* offset size field */
if (gtp_h->o == 1) {
offset = ntohs(*(uint16_t *)(pkt->data + proto_offset));
//ProtStackFrmDisp(pkt->stk, TRUE);
proto_offset += (offset + 2);
}
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
val.uint16 = tunnel;
ProtInsAttr(frame, tunnel_id, &val);
val.uint16 = session;
ProtInsAttr(frame, session_id, &val);
val.uint16 = 3; /* forced to be PPP */
ProtInsAttr(frame, proto_id, &val);
/* pdu */
pkt->data += proto_offset;
pkt->len -= proto_offset;
return pkt;
#endif
}
static packet* IpDissector(packet *pkt)
{
pstack_f *frame;
ftval val;
struct iphdr *ip;
unsigned short checksum_v;
size_t iphdr_len;
size_t ip_len;
if (sizeof(struct iphdr) > pkt->len) {
LogPrintf(LV_WARNING, "IP hedear packet dimension overflow the real dimension of packet");
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
ip = (struct iphdr *)pkt->data;
/* IPv- or IPv4 */
if (ip->version != 4) {
if (ip->version == 6 && ipv6_id != -1)
return ProtDissecPkt(ipv6_id, pkt);
LogPrintf(LV_WARNING, "IP verision %i without dissector", ip->version);
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
/* IPv4 */
iphdr_len = ip->ihl << 2;
ip_len = ntohs(ip->tot_len);
/* check consistence and checksum */
if (ip_len > pkt->len) {
//LogPrintf(LV_WARNING, "IP packet dimension overflow the real dimension of packet (%i>%i)", ip_len, pkt->len);
LogPrintf(LV_WARNING, "IP packet dimension overflow the real dimension of packet");
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
#if (XPL_DIS_IP_CHECKSUM == 0)
if (ip_len <= iphdr_len) {
LogPrintf(LV_WARNING, "Bogus IP length (%i, less than header length 20)", ip_len);
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
checksum_v = ip_fast_csum((unsigned char *)ip, ip->ihl);
if (checksum_v != 0) {
LogPrintf(LV_WARNING, "IP packet chechsum error (0x%x != 0x%x)", checksum_v, ip->check);
//ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
#else
if (ip_len <= iphdr_len) {
ip_len = pkt->len;
}
#endif
/* fragment ip */
if (ip->frag_off != 0 && ip->frag_off != 0x40) {
#warning we have to be implement the fragment ip
LogPrintf(LV_WARNING, "IP packet fragment 0x%x (%i)", ip->frag_off, ntohs(ip->frag_off)<<3);
ProtStackFrmDisp(pkt->stk, TRUE);
PktFree(pkt);
return NULL;
}
/* new frame */
frame = ProtCreateFrame(prot_id);
ProtSetNxtFrame(frame, pkt->stk);
pkt->stk = frame;
/* set attribute */
val.uint8 = ip->protocol;
ProtInsAttr(frame, proto_id, &val);
#ifdef XPL_X86
val.uint32 = ip->saddr;
#else
val.uint32 = Emb32(&ip->saddr);
#endif
ProtInsAttr(frame, src_id, &val);
#ifdef XPL_X86
val.uint32 = ip->daddr;
#else
val.uint32 = Emb32(&ip->daddr);
#endif
ProtInsAttr(frame, dst_id, &val);
val.uint32 = (pkt->data - pkt->raw);
ProtInsAttr(frame, offset_id, &val);
#if SNIFFER_EVASION
val.uint8 = ip->ttl;
ProtInsAttr(frame, ttl_id, &val);
val.uint16 = ntohs(ip->id);
ProtInsAttr(frame, id_id, &val);
#endif
/* pdu */
pkt->data += iphdr_len;
pkt->len = ip_len - iphdr_len;
return pkt;
}
