使用raw_socket进行双网口数据转发

#include <stdio.h> #include <stdlib.h> #include <strings.h> #include <sys/types.h> #include <sys/socket.h> #include <unistd.h> #include <linux/in.h> #include <signal.h> /************************************************************************/ /************************************************************************/ /************************************************************************/ /********************raw socket******************************************/ #include <sys/ioctl.h> // ioctl #include <net/if.h> // ifreq #include <string.h> // strcpy #include <linux/if_packet.h> // sockaddr_ll #include <sys/msg.h> #include <sys/ipc.h> #include <fcntl.h> #include <errno.h> #include <netinet/if_ether.h> #define BOOL unsigned char #define U8 unsigned char #define U16 unsigned short #define TRUE 1 #define FALSE 0 #define MAX_LENGTH_MSG 2000 typedef enum { RAW_MSG = 1, //eth0: RAW message 1 IP_MSG = 2 //eth1: IP message } MSGTYPE; struct s_msg { long type; U8 mtext[MAX_LENGTH_MSG]; }; static unsigned char if_mac[6]; struct sockaddr_ll sll; static int fd; char *rawPort = "eth1"; static unsigned char dstIP[4] = {0xc0, 0xa8, 0x3C, 0x0c}; static unsigned char dstIP_client[4] = {0xc0, 0xa8, 0x3C, 0x0e}; /*************************message queue operation*******************************/ int msgKey = 0x1240; int msgID = 0; int receiveMSG(U8 *txBuf, long msgType) { struct s_msg msg; int size = msgrcv(msgID, &msg, MAX_LENGTH_MSG, msgType, IPC_NOWAIT); if (size > 0) memcpy(txBuf, msg.mtext, size); return size; } int receiveFisrtMSG(U8 *txBuf, long *msgType) { struct s_msg msg; int size = msgrcv(msgID, &msg, MAX_LENGTH_MSG, 0, 0/*IPC_NOWAIT*/); *msgType = msg.type; if (size > 0) memcpy(txBuf, msg.mtext, size); return size; } int sendMSG(U8 *txBuf, int size, long msgType) { struct s_msg msg; msg.type = msgType; memcpy(msg.mtext, txBuf, size); return msgsnd(msgID, &msg, size, 0); } /*******************************************************************************/ void DispalyBuf( U8 *buf,U16 size) { int i; for(i=0;i<size;i++) { if((0==i%16)&&(i>0)) printf("\n"); printf("%02x ",buf[i]); } printf("\n"); } BOOL InitEtherNetIf( ) { struct ifreq req; int flag = 1; if ( (fd = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL) ) ) < 0 ) { printf( "failed to create raw socket!\n" ); return FALSE; } memset(&sll, 0, sizeof(sll)); sll.sll_family = PF_PACKET; sll.sll_protocol = htons(ETH_P_ALL); strcpy( req.ifr_name, rawPort ); if ( ioctl( fd, SIOCGIFFLAGS, &req ) < 0 ) { printf( "failed to do ioctl!" ); return FALSE; } req.ifr_flags |= IFF_PROMISC; if ( ioctl( fd, SIOCSIFFLAGS, &req ) < 0 ) { printf( "failed to set eth0 into promisc mode!" ); return FALSE; } if ( ioctl( fd, SIOCGIFHWADDR, &req ) < 0 ) { printf( "failed to get IF hw address!" ); return FALSE; } memcpy( if_mac, req.ifr_hwaddr.sa_data, 6 ); if ( ioctl( fd, SIOCGIFINDEX, &req ) < 0 ) { printf( "failed to get IF hw address!" ); return FALSE; } /* if (flag = fcntl(fd, F_GETFL, 0) < 0) printf("get flag error!\n"); if (fcntl(fd, F_SETFL, flag | O_NONBLOCK) < 0) { printf("failed to set non block!"); return FALSE; } */ sll.sll_ifindex = req.ifr_ifindex; sll.sll_halen = ETH_ALEN; if (bind(fd, (struct sockaddr *)&sll, sizeof(sll)) == -1) { printf("bind raw socket failed!\n"); return FALSE; } return TRUE; } BOOL GetPacket( U8 *buf, U16 *len) { int length = 0; length = recvfrom( fd, buf, 2000, 0, NULL, NULL ); if ( length < 0 ) { return 0; } else { *len = length; return 1; } } BOOL SendPacket( U8 *buf, U16 len) { if (write(fd, buf, len) != len) { printf( "failed to send to RAW socket! [%d]\r\n", errno ); return 0; } return 1; } /*******************************************************************************/ void sig_proccess(int signo) { printf("Catch a exit signal\n"); exit(0); } void sig_pipe(int sign) { printf("Catch a SIGPIPE signal\n"); /* 释放资源 */ } #define PORT 8888 /* 侦听端口地址 */ #define BACKLOG 2 /* 侦听队列长度 */ // process socket recieved frame void processRx(U8 *buf, U16 size) { U16 ethnetType = (buf[12]<<8) + buf[13]; unsigned char *dstMsgIP = buf + 30; if (ethnetType == 0x0800) {//IP datagram: send the datagram to the TCP tunnel dstMsgIP = buf + 30; } else if (ethnetType == 0x0806) {// ARP frame dstMsgIP = buf + 38; } if (sendMSG(buf, size, RAW_MSG) < 0) { printf("send ip msg to queue failed!\n"); } } /* 服务器对客户端的处理 */ void process_conn_server(int s) { ssize_t size = 0; char buffer[1024]; /* 数据的缓冲区 */ for(;;) {/* 循环处理过程 */ //server receive message /* 从套接字中读取数据放到缓冲区buffer中 */ size = read(s, buffer, 1024); if(size == 0){/* 没有数据 */ return; } else { printf("server RX[%d]:\n", size); DispalyBuf(buffer, size); processRx(buffer, size); } // server send message size = receiveMSG(buffer, IP_MSG); if (size > 0) { printf("Server Tx[%d]:\n", size); DispalyBuf( buffer, size); if (write(s, buffer, size) < 0) { printf("server send message failed!\n"); } } } } #define CONCURRENT_MAX 1 int client_fds[CONCURRENT_MAX] = {0}; // process raw socket messages BOOL ProcessBuf1( U8 *buf,U16 size) { U16 ethnetType = (buf[12]<<8) + buf[13]; unsigned char *dstMsgIP = buf + 30; if (ethnetType == 0x0800) {//IP datagram: send the datagram to the TCP tunnel } else if (ethnetType == 0x0806) { dstMsgIP = buf + 38; } if (memcmp(dstMsgIP, dstIP, 4) == 0) { printf("raw rx[%d]\n", size); DispalyBuf( buf, size); } else return FALSE; return TRUE; } void ProcessBuf( U8 *buf,U16 size) { U16 ethnetType = (buf[12]<<8) + buf[13]; unsigned char *dstMsgIP = buf + 30; U8 txBuf[MAX_LENGTH_MSG] = {0}; U16 txLen = size; int index = 0; if (ethnetType == 0x0800) {//IP datagram: send the datagram to the TCP tunnel U16 IPHeadLen = buf[14] & 0x0F; U16 IPLen = (buf[16] << 8) + buf[17]; unsigned char *tcpStart = buf + 14 + IPHeadLen * 4; unsigned char *dataStart = tcpStart + 20; // tcp head length is 20; U16 dataSize = IPLen - IPHeadLen * 4 - 20; U16 headLen = 14 + IPHeadLen * 4 + 20; if (memcmp(dstMsgIP, dstIP, 4) == 0) { printf("raw rx data[%d]: \n", dataSize); DispalyBuf(dataStart, dataSize); printf("raw frame\n"); DispalyBuf(buf, size); if (dataStart[0] == 0x02) {// security process memcpy(txBuf, buf, headLen); txBuf[headLen] = 0x86; txBuf[headLen + 1] = dataSize + 1; memcpy(txBuf + headLen + 2, buf + headLen, dataSize); U8 checkSum = 0; for (index = 0; index < dataSize; ++index) { checkSum ^= dataStart[index]; } txLen = size + 3; txBuf[headLen + dataSize + 2] = checkSum; printf("Client Tx security data: \n"); DispalyBuf(txBuf, txLen); } else {// no process memcpy(txBuf, buf, size); txLen = size; printf("Client Tx not security data:\n "); DispalyBuf(txBuf, txLen); } if ( write(client_fds[0], txBuf, txLen) < 0 ) { printf("client send message failed!\n"); // 发送给服务器 } } } else if (ethnetType == 0x0806) { dstMsgIP = buf + 38; if (memcmp(dstMsgIP, dstIP, 4) == 0) { // printf("raw rx[%d]:\n", size); // DispalyBuf( buf, size); } if ( write(client_fds[0], buf, size) < 0 ) { printf("client send message failed!\n"); // 发送给服务器 } } } void ProcessTCPFrame( U8 *buf,U1