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