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; }