netfilter_queue修改网络报文示例

使用时需按情况修改，并添加如iptables -A INPUT -j NFQUEUE --queue-num 0之类的iptables规则                                              

#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <fcntl.h>
#include <errno.h>
#include <termios.h>
#include <netinet/in.h>
#include <linux/types.h>
#include <linux/netfilter.h> /* for NF_ACCEPT */
#include <linux/ip.h>
#include <linux/udp.h>
#include <linux/if_ether.h>
//#include <arpa/inet.h>
#include <asm/byteorder.h>

#include <libnetfilter_queue/libnetfilter_queue.h>
#include <libnetfilter_queue/libnetfilter_queue_udp.h>

#ifdef __LITTLE_ENDIAN
#define IPQUAD(addr) \
((unsigned char *)&addr)[0], \
((unsigned char *)&addr)[1], \
((unsigned char *)&addr)[2], \
((unsigned char *)&addr)[3]
#else
#define IPQUAD(addr) \
((unsigned char *)&addr)[3], \
((unsigned char *)&addr)[2], \
((unsigned char *)&addr)[1], \
((unsigned char *)&addr)[0]
#endif

#define IP_HDR_LEN 20
#define UDP_HDR_LEN 8
#define TOT_HDR_LEN 28

struct rtphdr
{
    __u8 cc:4;
    __u8 x:1;
    __u8 p:1;
    __u8 v:2;
    __u8 pt:7;
    __u8 m:1;

    __u16 seq;
    __u16 ts;
    __u32 ssrc;
    __u32 csrc[1];
};

/******************************************************/
long checksum(unsigned short *addr,unsigned int count) {
    register long sum = 0;
    while(count>1){
        sum += *addr++;
        count -= 2;
    }
    if(count > 0)
        sum += *(unsigned char *)addr;

    while(sum>>16)
        sum = (sum &0xffff) + (sum>>16);

    return ~sum;
}

static u_int16_t ip_checksum(struct iphdr* iph) {
    return checksum((unsigned short *)iph,iph->ihl<<2);
}

static void set_ip_checksum(struct iphdr* iph) {
    iph->check = 0;
    iph->check = checksum((unsigned short *)iph,iph->ihl<<2);;
}

static void set_udp_checksum(struct iphdr *pIph, unsigned short *ipPayload) {
    register unsigned long sum = 0;
    struct udphdr *udphdrp = (struct udphdr*)(ipPayload);
    unsigned short udpLen = htons(udphdrp->len);

    sum += (pIph->saddr>>16)&0xFFFF;
    sum += (pIph->saddr)&0xFFFF;
    sum += (pIph->daddr>>16)&0xFFFF;
    sum += (pIph->daddr)&0xFFFF;
    sum += htons(IPPROTO_UDP);
    sum += udphdrp->len;

    udphdrp->check = 0;
    while(udpLen > 1){
        sum += *ipPayload++;
        udpLen -= 2;
    }

    if(udpLen > 0){
        sum += ((*ipPayload)&htons(0xFF00));
    }

    while(sum>>16){
        sum = (sum & 0xFFFF) + (sum>>16);
    }

    sum = ~sum;
    udphdrp->check = ((unsigned short)sum == 0x0000)?0xFFFF:(unsigned short)sum;
}
/*********************************************************/

static int is_target_packet(char *data){
    struct iphdr *iph;
    struct udphdr *udph;
    struct rtphdr *rtph;
    int i;

    //char* mydata;
    unsigned int dst_port,src_port;
    int result = 0;

    iph = (struct iphdr *)data;
    if(iph)
    {
        if(iph->protocol == IPPROTO_UDP){
            udph = (struct udphdr *)(data+20);
            dst_port = ntohs(udph->dest);
            src_port = ntohs(udph->source);
            if(dst_port != 5060 && src_port != 5060){
                if(src_port >= 1024 && dst_port >= 1024){
                    if(src_port%2 == 0 && dst_port%2 == 0){
                    i = ntohs(iph->tot_len)-28;
                        if(i > 12){
                            rtph = (struct rtphdr *)(data+28);
                            if(rtph->v == 2){
                                unsigned int j = rtph->cc;
                                unsigned int hdrlen = j*4+12;
                                if(i > hdrlen){
                                    result = 1;
                                }
                            }
                        }
                    }
                }
            }
        }
    }
    return result;
}

static void encrypt_payload(char **data) {
}

/* returns packet id */
static u_int32_t edit_pkt (struct nfq_data *tb)
{
    int id = 0;
    struct nfqnl_msg_packet_hdr *ph;
    struct nfqnl_msg_packet_hw *hwph;
    u_int32_t mark,ifi;
    int ret,n,rtp_hdr_len;
    char *data;
    char *rtp_payload;
    char *ip_payload;
    struct iphdr *iph;
    struct udphdr *udph;
    struct rtphdr *rtph;

    ret = nfq_get_payload(tb, &data);
    if (ret >= 0)
        printf("payload_len=%d ", ret);

    n = is_target_packet(data);
    if(n == 1){
        iph = (struct iphdr *)(data);
        ip_payload = data + IP_HDR_LEN;
        udph = (struct udphdr *)(data + IP_HDR_LEN);
        rtph = (struct rtphdr *)(data + TOT_HDR_LEN);
        rtp_hdr_len = (rtph->cc)*4+12;
        rtp_payload = data + TOT_HDR_LEN +rtp_hdr_len;
        encrypt_payload(&rtp_payload);
        set_ip_checksum(iph);
        set_udp_checksum(iph,(unsigned short *)ip_payload);
    }

    fputc('\n', stdout);

    return id;
}


static int cb(struct nfq_q_handle *qh, struct nfgenmsg *nfmsg,
          struct nfq_data *nfa, void *data)
{
    u_int32_t id = edit_pkt(nfa);
    printf("entering callback\n");
    return nfq_set_verdict(qh, id, NF_ACCEPT, 0, NULL);
}

int main(int argc, char **argv)
{
    struct nfq_handle *h;
    struct nfq_q_handle *qh;
    int fd;
    int rv;
    char buf[4096] __attribute__ ((aligned));

    printf("opening library handle\n");
    h = nfq_open();
    if (!h) {
        fprintf(stderr, "error during nfq_open()\n");
        exit(1);
    }

    printf("unbinding existing nf_queue handler for AF_INET (if any)\n");
    if (nfq_unbind_pf(h, AF_INET) < 0) {
        fprintf(stderr, "error during nfq_unbind_pf()\n");
        exit(1);
    }

    printf("binding nfnetlink_queue as nf_queue handler for AF_INET\n");
    if (nfq_bind_pf(h, AF_INET) < 0) {
        fprintf(stderr, "error during nfq_bind_pf()\n");
        exit(1);
    }

    printf("binding this socket to queue '0'\n");
    qh = nfq_create_queue(h, 0, &cb, NULL);
    if (!qh) {
        fprintf(stderr, "error during nfq_create_queue()\n");
        exit(1);
    }

    printf("setting copy_packet mode\n");
    if (nfq_set_mode(qh, NFQNL_COPY_PACKET, 0xffff) < 0) {
        fprintf(stderr, "can't set packet_copy mode\n");
        exit(1);
    }

    fd = nfq_fd(h);

    while ((rv = recv(fd, buf, sizeof(buf), 0)) && rv >= 0) {
        printf("pkt received\n");
        nfq_handle_packet(h, buf, rv);
    }

    printf("unbinding from queue 0\n");
    nfq_destroy_queue(qh);

#ifdef INSANE
    /* normally, applications SHOULD NOT issue this command, since
     * it detaches other programs/sockets from AF_INET, too ! */
    printf("unbinding from AF_INET\n");
    nfq_unbind_pf(h, AF_INET);
#endif

    printf("closing library handle\n");
    nfq_close(h);

    exit(0);
}