过滤gsm的c

#include <stdio.h>
#include <stdlib.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <errno.h>
#include <string.h>
#include <sys/ioctl.h>
#include <linux/if_ether.h>
#include <linux/in.h>
#include <net/if.h>
#include <linux/filter.h>

char buf[2048];
//char result[]={0x09,0x01,0x3f,0x01,0x01,0x08,0x91,0x68,0x31,0x08,0x10,0x00,0x85,0xf8,0x00,0x32,0x24,0x0d,0x91,0x68,0x81,0x16,0x52,0x79,0x16,0xf2,0x00,0x08,0x31,0x11,0x60,0x71,0x83,0x21,0x23,0x1e,0x62,0x11,0x53,0xbb,0x96,0xcd,0x54,0x8c,0x5b,0xab,0xff,0x0c,0x4f,0x60,0x89,0x81,0x66,0x2f,0x57,0x50,0x57,0x30,0x94,0xc1,0x54,0x4a,0x8b,0xc9,0x62,0x11};
char result[2048];
char Two_2_3[]={};

int d_deal(int num)
{
    int di=0;
    int m = 0;
    int m_len = 0;
    int dst_len = 0;
    printf("T_src:");
    for ( di=7;di<=13;di++)
    {
        printf("%02x",((result[di] << 4 ) & 0xf0) | ((result[di] >> 4) & 0xf)); // change the positon of result[7] 68=>86
    }
    printf("\t");
    m_len=(int)((result[17]+1)/2);
    printf("T_dst:");
    dst_len=19+m_len;
    for (di=19; di < dst_len; di++)
    {
        printf("%02x",((result[di] << 4 ) & 0xf0) | ((result[di] >> 4) & 0xf)); // change the positon of result[7] 68=>86
    }
    printf("\ttext:");
    for (di=dst_len + 10; di < num; di+=2)
    {
        if(result[di] == 0x00)
        {
            Two_2_3[m] = result[di+1];
            m++;
        }
        else
        {
        Two_2_3[m]=(0xe0 | (result[di] & 0xf0) >> 4);
        Two_2_3[m+1] = (0x80 | (result[di] & 0x0f) << 2 | (result[di+1] & 0xc0) >> 6);
        Two_2_3[m+2] = (0x80 | result[di+1] & 0x3f);
        m+=3;
        }
    }
    printf("%s\n",Two_2_3);

}


int main(int argc,char **argv)
{
    int sock,n,i;
    int j = 0;
    int m = 0;
    int tal_num = 0;
    int d_len =0;
    unsigned char *iphead,*ethhead;
    struct ifreq ethreq;
//    struct sigaction sighandle;

    if ((sock = socket(PF_PACKET,SOCK_RAW,htons(ETH_P_IP))) < 0)
    {
        perror("socket failed");
        exit(1);
    }


    /*set promiscuos mode*/
    strncpy(ethreq.ifr_name, "wlp6s0", IFNAMSIZ);
    if (ioctl(sock,SIOCGIFFLAGS,&ethreq) == -1)
    {
        perror("ioctl failed");
        close(sock);
        exit(1);
    }
    ethreq.ifr_flags |= IFF_PROMISC;
    if (ioctl(sock,SIOCSIFFLAGS,&ethreq) == -1)
    {
        perror("ioctl failed");
        close(sock);
        exit(1);
    }

    struct sock_filter bpf_code[] = {
        { 0x28, 0, 0, 0x0000000c },
        { 0x15, 0, 4, 0x000086dd },
        { 0x30, 0, 0, 0x00000014 },
        { 0x15, 0, 11, 0x00000011 },
        { 0x28, 0, 0, 0x00000038 },
        { 0x15, 8, 9, 0x00000035 },
        { 0x15, 0, 8, 0x00000800 },
        { 0x30, 0, 0, 0x00000017 },
        { 0x15, 0, 6, 0x00000011 },
        { 0x28, 0, 0, 0x00000014 },
        { 0x45, 4, 0, 0x00001fff },
        { 0xb1, 0, 0, 0x0000000e },
        { 0x48, 0, 0, 0x00000010 },
        { 0x15, 0, 1, 0x00000035 },
        { 0x6, 0, 0, 0x00000051 },
        { 0x6, 0, 0, 0x00000000 },
    };
    struct sock_fprog filter;
    filter.len = sizeof(bpf_code)/sizeof(bpf_code[0]);
    filter.filter = bpf_code;

    //sighandle.sa_flags = 0;
    //sighandle.sa_handler = sig_handler;
//    sigemptyset(&sighandle.sa_mask);

//    sigaction(SIGTERM,&sighandle,NULL);
//    sigaction(SIGINT,&sighandle,NULL);
//    sigaction(SIGQUIT,&sighandle,NULL);


    if (setsockopt(sock, SOL_SOCKET, SO_ATTACH_FILTER, &filter, sizeof(filter)) < 0)
    {
        perror("setsockopt failed");
        close(sock);
        exit(1);
    }

    while (1)
    {
        int is_last = 0;
        n = recvfrom(sock, buf, sizeof(buf), 0, NULL, NULL);
        if (n < (14 + 20 + 8))
        {
            printf("invalid packet\n");
            continue;
        }
        printf("%d bytes recieved\n",n);
        ethhead = buf;
    //    printf("dst port: %02x%02x\n",ethhead[36],ethhead[37]);
    //    printf("channel type: %02x\n",ethhead[54]);
    //    if ( 0xf == 15 ) printf("find ok\n%02x\n", (0x03 >> 4) | (0x03 << 4));
        if (ethhead[54] == 0x08 && (ethhead[59] & 0x1) == 0x0 ) //the last bit of 59 byte decide if frame
        {
            d_len = (ethhead[60] & 0x2)?20:(is_last++,((ethhead[60] >> 2) & 0x3f));
            tal_num+=d_len;
            for (j=1; j <= d_len; m++,j++)//read len data
            {
                result[m]=ethhead[60+j];
            }
            if (is_last) {m = 0;d_deal(tal_num);tal_num = 0;}
        }
    }
}

bpf过滤的是upd 53 的流量，  gsm请过滤udp 4729 进行替换。 tcpdump udp and dst port 4729 -s 81 -dd
注：   wireshark中的信息部分，是2字节表示一个汉字，但是c语言中3个字节表示一个汉字。
1110 xxxx  10xxxxxx 10xxxxxx  ,  左边的16个x表示 wireshark中的2个字节。