int decode_datapkg_hdr(char* pBuffer)
{
int *aiLen = (int*)(pBuffer+8);
int iDataPkgLen = aiLen[0]+16;
char *pBuff = pBuffer+16;
pBuff += decode_datalink_hdr(pBuff);
pBuff += decode_ip_hdr(pBuff);
pBuff += decode_tcp_hdr(pBuff);
pBuff += parse_use_data(pBuff,iDataPkgLen-(pBuff-pBuffer));
return iDataPkgLen;
}
/* Takes a pointer to the beginning of a packet and returns 0 or -1
* on failure.
*/
/* virtual */ SysStatus
PacketFilterRoot::filterFind(char *packet, uval *endpt_arg)
{
struct filter_node *root;
struct filter_node_object *node_obj;
char *packet_pos;
struct etherhdr *eth_hdr;
struct iphdr *ip_hdr;
struct tcphdr *tcp_hdr;
struct udphdr *udp_hdr;
packet_pos = packet;
if (filterRoot == NULL || packet == NULL) {
return -1;
}
filterLock.acquire();
root = filterRoot;
/* Check ethernet type */
eth_hdr = (struct etherhdr *)packet_pos;
packet_pos += sizeof(struct etherhdr);
// dprintf("Checking ether prototype: %#x\n", ntohs(eth_hdr->ether_type));
node_obj = filterFindNodeObject(root, eth_hdr->ether_type, 0);
if (node_obj == NULL) {
goto error;
}
/* Continue if this is an IP packet */
if (eth_hdr->ether_type == HTONS(ETHER_TYPE_IP)) {
ip_hdr = (struct iphdr *)packet_pos;
packet_pos += (ip_hdr->ip_hl * 4);
/* Make sure we are dealing with IPv4 */
if (ip_hdr->ip_v != 4) {
goto error;
}
/* Check IP protocol */
//printf("Checking IP Protocol: %#x\n", ip_hdr->ip_p);
root = node_obj->next_node;
node_obj = filterFindNodeObject(root, ip_hdr->ip_p, 0);
if (node_obj == NULL) {
goto error;
}
if (ip_hdr->ip_p == IP_PROTOCOL_TCP) {
tcp_hdr = (struct tcphdr *)packet_pos;
/* Check TCP destination port */
root = node_obj->next_node;
node_obj = filterFindNodeObject(root, tcp_hdr->tcp_dest, 0);
if (node_obj == NULL) {
goto error;
}
/* Check TCP source port */
root = node_obj->next_node;
node_obj = filterFindNodeObject(root, tcp_hdr->tcp_source, 0);
if (node_obj == NULL) {
goto error;
}
} else if (ip_hdr->ip_p == IP_PROTOCOL_UDP) {
udp_hdr = (struct udphdr *)packet_pos;
/* Check UDP destination port */
root = node_obj->next_node;
node_obj = filterFindNodeObject(root, udp_hdr->udp_dest, 0);
if (node_obj == NULL) {
goto error;
}
/* Check UDP source port */
root = node_obj->next_node;
node_obj = filterFindNodeObject(root, udp_hdr->udp_source, 0);
if (node_obj == NULL) {
goto error;
}
}
/* Check IP source addr */
root = node_obj->next_node;
node_obj = filterFindNodeObject(root, ip_hdr->ip_source, 0);
if (node_obj == NULL) {
goto error;
}
}
/* If we got this far then there this packet matched a filter
* return the endpoint id
*/
dprintf("Found match\n");
#if DECODE_PACKET > 0
decode_ip_hdr(ip_hdr);
decode_tcp_hdr(tcp_hdr);
#endif /* #if DECODE_PACKET > 0 */
if (node_obj->callback != NULL) {
node_obj->callback(node_obj->callback_arg, packet);
}
// FIXME...
*endpt_arg = node_obj->callback_arg;
filterLock.release();
return 0;
error:
filterLock.release();
return -1;
}
