// function callback for packet processing
// ---------------------------------------
static int nfqueue_cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
struct nfq_data *nfa, void *data) {
struct nfqnl_msg_packet_hdr *ph;
int id_protocol = 0, id = 0, len = 0;
unsigned char *full_packet; // get data of packet (payload)
unsigned char *new_packet = NULL;
ph = nfq_get_msg_packet_hdr(nfa);
if (ph) {
len = nfq_get_payload(nfa, &full_packet);
id = ntohl(ph->packet_id);
id_protocol = identify_ip_protocol(full_packet);
if (id_protocol == IPPROTO_UDP) { /* Only UDP packets */
printf("Packet from %s:%d", get_src_ip_str(full_packet), get_udp_src_port(full_packet));
printf(" to %s:%d\n", get_dst_ip_str(full_packet), get_tcp_dst_port(full_packet));
/* Process packet... */
new_packet = process_packet(full_packet, &len, ph->hook);
}
// let the packet continue on. NF_ACCEPT will pass the packet
// -----------------------------------------------------------
if (new_packet) {
nfq_set_verdict(qh, id, NF_ACCEPT, len, new_packet);
free(new_packet);
}
else
nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
} else {
printf("NFQUEUE: can't get msg packet header.\n");
return ERR; // from nfqueue source: 0 = ok, >0 = soft error, <0 hard error
}
return OK;
}
static int nfqueue_cb(
struct nfq_q_handle *qh,
struct nfgenmsg *nfmsg,
struct nfq_data *nfa,
void *data) {
char *pkt;
struct nfqnl_msg_packet_hdr *ph;
ph = nfq_get_msg_packet_hdr(nfa);
if ( ph ) {
int id = 0, size = 0;
id = ntohl(ph->packet_id);
size = nfq_get_payload(nfa, &pkt);
struct ip *iph = (struct ip *) pkt;
int id_protocol = identify_ip_protocol(pkt);
int dport = get_udp_dst_port(pkt);
int x = sizeof (struct ip) + sizeof (struct udphdr);
/* packets we are interested in are UDP multicast to 239.255.255.250:1900
* and start with a data string M-SEARCH
*/
if ( (dport == 1900) && (id_protocol == IPPROTO_UDP)
&& (ssdp.sin_addr.s_addr == iph->ip_dst.s_addr) ) {
/* get the index that the packet came in on */
u_int32_t idx = nfq_get_indev(nfa);
int i = 0;
for ( ;i < n_nfqix ; i++) {
if ( nfqix[i] == idx ) {
struct udphdr *udp = (struct udphdr *) (pkt + sizeof(struct ip));
char *dd = pkt + x;
struct sockaddr_in sendername;
sendername.sin_family = AF_INET;
sendername.sin_port = udp->source;
sendername.sin_addr.s_addr = iph->ip_src.s_addr;
/* printf("pkt found %s\n",dd);*/
ProcessSSDPData (sudp, dd, size - x,
&sendername, (unsigned short) 5555);
}
}
}
nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
} else {
syslog(LOG_ERR,"nfq_get_msg_packet_hdr failed");
return 1;
/* from nfqueue source: 0 = ok, >0 = soft error, <0 hard error */
}
return 0;
}
