winevl_bridge.c

该代码为linux下通过usb驱动实现的无线网络驱动程序,在2.6.18的内核下调试通过

#include <stdio.h>
#include <string.h>
#include <sys/time.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <netinet/tcp.h>
#include <netdb.h>
#include <arpa/inet.h>
#include <stdlib.h>
#include <time.h>
#include <ctype.h>
#include <unistd.h>
#include <signal.h>
#include <linux/if_ether.h>
#include <sys/ioctl.h>

#include <linux/if.h>
#include <wait.h>

#define MAXLINE				2560

/* Definition of commands */
#define CMD_QUERY_DEVICE_STATUS		0x10
#define CMD_DEVICE_STATUS_RESPONSE	0x11
#define CMD_QUERY_INFORMATION		0x20
#define CMD_QUERY_INFORMATION_RESPONSE	0x21
#define CMD_SET_INFORMATION		0x30
#define CMD_SET_INFORMATINO_RESPONSE	0x31

#define STATUS_SUCCESS			0x0001
#define STATUS_FAIL			0x0000

#define ZDPRODUCTIOCTL			0x89FA
#define ZDAPIOCTL   0x89F0
#define ZD_IOCTL_REG_READ           0x01
#define ZD_IOCTL_REG_WRITE           0x02



struct zdap_ioctl {
    unsigned short cmd;                /* Command to run */
    unsigned long addr;                /* Length of the data buffer */
    unsigned long value;               /* Pointer to the data buffer */
    unsigned char data[0x100];
};


typedef unsigned char  u8;
typedef unsigned short u16;
typedef unsigned int   u32;

struct  zd_point
{
	caddr_t       pointer;
	__u16         length;
};

struct  zdreq
{
	union
	{
		char    ifrn_name[IFNAMSIZ];
	} ifr_ifrn;
	union
	{
		struct	zd_point data;
	} u;
};
typedef struct oid_wrap
{
    u16 request;
    u16 seq;

    union
    {
        struct {
            u16 status;
        } dev;

        struct {
            u32 oid;
            u32 status;
            u32 length;
            u8  data[512];
        } info;
    } u;

} oid_wrap_t;
typedef struct   _ZD_CUSTOM_STRUCT
{
    u32 ZDCustomLength;
    u32 ZDFuncId;
    u32 DataBuffer[2];
} ZD_CUSTOM_STRUCT, *PZD_CUSTOM_STRUCT;


#define ZD_GENERIC_OID_HDR_LEN \
((int) (&((struct oid_wrap *) 0)->u.info.data))

#define ZD_DEV_STATUS_LEN			6

char *dev = NULL;
int msock;
int myPID=0;
int myERRNO=0;
int regwrite_ioctl(char *dev_name, int mac_reg , int value)
{
    int sock;
    struct ifreq req;
    char *action = NULL;
    struct zdap_ioctl zdreq;


    memset(zdreq.data, 0, sizeof(zdreq.data));
    sock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
    if (sock < 0) {
        printf("Socket Error\n");
        exit(1);
    }

    strcpy(req.ifr_name, dev_name);
    zdreq.addr = mac_reg;
    zdreq.value = value;

    zdreq.cmd = ZD_IOCTL_REG_WRITE;
    req.ifr_data = (char *)&zdreq;
    if (ioctl(sock, ZDAPIOCTL, &req) < 0) {
        printf("IOCTL fail\n");
        close(sock);
        return -1;
    }
    close(sock);
    return 0;
}


int regread_ioctl(char *dev_name, int mac_reg , int *result)
{
    int sock;
    int addr, value;
    struct ifreq req;
    char *action = NULL;
    struct zdap_ioctl zdreq;


	memset(zdreq.data, 0, sizeof(zdreq.data));
    sock = socket(AF_INET, SOCK_RAW, IPPROTO_RAW);
    if (sock < 0) {
        printf("Socket Error\n");
        exit(1);
    }

    strcpy(req.ifr_name, dev_name);
    zdreq.addr = 0;
    zdreq.value = 0;
    //sscanf(argv[3], "%x", &addr);
    //sscanf(argv[4], "%x", &value);
    zdreq.addr = mac_reg;
    //zdreq.value = value;

	
	zdreq.cmd = ZD_IOCTL_REG_READ;
	req.ifr_data = (char *)&zdreq;
    if (ioctl(sock, ZDAPIOCTL, &req) < 0) {
		printf("IOCTL fail\n");
		close(sock);
        return -1;
    }
	else {
		*result = *(int *)zdreq.data;
		close(sock);
		return 0;
	}
	close(sock);
    return 0;
}

int do_ioctl(struct oid_wrap *param, int len)
{
	int sock;
	struct zdreq ireq;
	int ret = 0;

	/* get a socket */
	sock = socket(AF_INET, SOCK_STREAM, 0);

	if(sock < 0 )
	{
		perror("Can't open device\n");
		return -1;
	}

	printf("dev: %s, size: %d\n", dev, strlen(dev));

	memset(&ireq, 0, sizeof(ireq));
	strcpy(ireq.ifr_name, dev);
	ireq.u.data.pointer =  (caddr_t) param;
	ireq.u.data.length = len;

	ret = ioctl(sock, ZDPRODUCTIOCTL, &ireq);

	if (ret != 0)
		perror("ioctl[ZDPRODUCTIOCTL]\n");
    
	close(sock);
	return ret;
}

void sigterm(int signo)
{
	if(signo == SIGINT) {
		/* close socket */
		close(msock);
	}

}

void hexdump(u8 *buf, int size)
{
	int ii;
	
	return;
	for(ii = 0; ii < size; ) {

		printf("%02x ", buf[ii]);

		if((++ii % 16) == 0)
			printf("\n");
	}
	printf("\n");
}

int main(int argc, char *argv[])
{
	int ret;
	int CR157=0;
	socklen_t cli_len;
	int ch;
	struct sockaddr_in serv, cli;
	char buf[MAXLINE];
	int i;
	int tmppid,stat=0;
	int RegCR157[7] = {0x00, 0xF0, 0xE0,0xD0, 0x10, 0x20, 0x30};
     
	fd_set afds, rfds;
	struct in_addr addr;
	char *pf[20]; //Buffer for deliver command to pktsnd
	int nsize;

	while((ch = getopt(argc, argv, "i:")) != -1)
	{
		switch(ch)
		{
		case 'i':
				dev = optarg;
				break;
		default:
				printf("argument: -%c not support\n", ch);
				break;
		}
	}

	if(dev == NULL) {
		printf("Usage: program -i <net iface>\n");
		exit(0);
	}

	memset(buf, 0, sizeof(buf));

	serv.sin_family = AF_INET;
	serv.sin_port = htons(1261);
	serv.sin_addr.s_addr = htonl(INADDR_ANY);

	if ((msock = socket(AF_INET, SOCK_DGRAM, 0)) < 0)
	{
		perror("socket error");
		exit(0);
	}

	ret = bind(msock, (struct sockaddr *) &serv, sizeof(serv));

	if (ret == -1) {
		perror("bind");
		exit(0);
	}

	FD_ZERO(&afds);
	FD_SET(msock, &afds);
	FD_SET(0, &afds);

	cli_len = sizeof(cli);
	for(i=0;i<20;i++) {
		pf[i] = (char *)malloc(100);
		if(pf[i] == NULL)
			printf("NULLLLLLLLLLLLLLLLLLLL\n");
	}
	while(1)
	{

		bcopy((char *)&afds, (char *) &rfds, sizeof(rfds));

		select(msock + 1, &rfds, 0, 0, NULL);
//		for(i=0;i<20;i++)
//			memset(pf[i],0,100);


		if(myPID != 0) {
			waitpid(myPID,&stat,WNOHANG);
			printf("WaitPID Result:%d\n", stat);
			if(stat == 0) myPID=0;
			stat = 0;
		}
        
		if(FD_ISSET(msock, &rfds))
		{
			int psize;
			oid_wrap_t *pbuf;
			nsize = recvfrom(msock, buf, MAXLINE, 0, (struct sockaddr *) &cli, &cli_len);

			printf("Receive data: %d\n", nsize);

			/* Parse the received packet */
			//pbuf = (oid_wrap_t *) (buf+42);
			//psize = nsize - 42;
			pbuf = (oid_wrap_t *) buf;
			psize = sizeof(oid_wrap_t);//nsize;
 
			ZD_CUSTOM_STRUCT *zdrd = (ZD_CUSTOM_STRUCT *)pbuf->u.info.data;
            if(pbuf->request == CMD_QUERY_INFORMATION  &&
               pbuf->u.info.oid == 0xFF129901       && //OID_ZD_CUSTOM
               zdrd->ZDFuncId == 0x88)  //ZD_K_PktSend
			{             
				pbuf->request = CMD_QUERY_INFORMATION_RESPONSE;
				pbuf->u.info.status = (myPID == 0?0:1);
                printf("CMD_QUERY_PKT_SEND : %d\n",pbuf->u.info.status);

                sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN , 0,
						(struct sockaddr *) &cli, cli_len);
                continue;

            }

			if(pbuf->request == CMD_SET_INFORMATION && 
			   pbuf->u.info.oid == 0xFF129901       && //OID_ZD_CUSTOM
			   zdrd->ZDFuncId == 0x88)              //ZD_K_PktSend
			{
				if(zdrd->ZDCustomLength< 18) {
					printf("The PktSend CMD is too short\n");
					return -1;
				}
				
				if(pf[0] == NULL)
					printf("dddddddddddddddddddddddd\n");
				strcpy(pf[0],"A"); //Useless
				strcpy(pf[1],dev); //interface
				sprintf(pf[2],"%d",zdrd->DataBuffer[0]); //Length
				sprintf(pf[3],"%d",zdrd->DataBuffer[1]); //Count
				sprintf(pf[4],"%d",zdrd->DataBuffer[2]); //Style
				sprintf(pf[5],"%x",((char *)zdrd->DataBuffer)[sizeof(u32)*3+0] & 0xff);
				sprintf(pf[6],"%x",((char *)zdrd->DataBuffer)[sizeof(u32)*3+1] & 0xff);
                sprintf(pf[7],"%x",((char *)zdrd->DataBuffer)[sizeof(u32)*3+2] & 0xff);
                sprintf(pf[8],"%x",((char *)zdrd->DataBuffer)[sizeof(u32)*3+3] & 0xff);
                sprintf(pf[9],"%x",((char *)zdrd->DataBuffer)[sizeof(u32)*3+4] & 0xff);
                sprintf(pf[10],"%x",((char *)zdrd->DataBuffer)[sizeof(u32)*3+5]& 0xff);
				printf("SendPkt Length:%s\n", pf[2]);
				printf("SendPkt Cnt:%s\n", pf[3]);
				printf("SendPkt Style:%s\n",pf[4]);
				printf("SendPkt MAC:%s %s %s %s %s %s\n",pf[5],pf[6],pf[7],pf[8],pf[9],pf[10]);
                pbuf->request = CMD_SET_INFORMATINO_RESPONSE;
                pbuf->u.info.length = 0;

				if(myPID == 0) {
					tmppid = fork();
					if(tmppid == 0) {
						int idx=0;
						if(( zdrd->DataBuffer[2] & 0x0000ff00 ) == 0x0100) {
							for(idx=0;idx<7;idx++) {
								printf("IQTest,CR157:0x%02x\n", RegCR157[idx]);
								regwrite_ioctl(dev, 628, 0xff & RegCR157[idx]);
								sprintf(pf[11],"%x",RegCR157[idx]);
								ret = pktsnd(12,(char **)pf);
							}
						}
						else {
							printf("CMD_SET_PKT_SEND Start Sending...\n");
							ret = pktsnd(12,(char **)pf);
							printf("CMD_SET_PKT_SEND End Sending...\n");
						}
						
						if(ret == -1)
							printf("pktsnd error exit\n");
						myERRNO = ret;
						return ret;
					}
					else if(tmppid == -1) {
                        pbuf->u.info.status = 1;
                        printf("CMD_SET_PKT_SEND Fail. Maybe out of memory\n");
                        sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN , 0,
                                (struct sockaddr *) &cli, cli_len);
                        continue;

					}
					else if(tmppid > 0){
						myPID = tmppid;
						pbuf->u.info.status = 0;
						printf("CMD_SET_PKT_SEND Successful\n");
						sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN , 0, 
								(struct sockaddr *) &cli, cli_len);
						continue;
					}
					else {
						printf("Unknown fork error code :%d\n", tmppid);
						continue;
					}
				}
				else {
					pbuf->u.info.status = 1;	
					printf("FAIL.CMD_SET_PKT_SEND while  already sending\n");
					sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN , 0, 
						(struct sockaddr *) &cli, cli_len);
					continue;
				}

			}

			switch(pbuf->request)
			{
			case CMD_QUERY_DEVICE_STATUS:
				printf("CMD_QUERY_DEVICE_STATUS: \n");

				/* Query the status of the device */
				pbuf->request = CMD_DEVICE_STATUS_RESPONSE;
				pbuf->u.dev.status = STATUS_SUCCESS;
				//sendto(msock, pbuf, ZD_DEV_STATUS_LEN, 0, (struct sockaddr *) &cli, cli_len);
				//hexdump((u8 *)pbuf, ZD_DEV_STATUS_LEN);

				hexdump((u8 *)pbuf, psize);

				break;
            
			case CMD_QUERY_INFORMATION:
				printf("CMD_QUERY_INFORMATION: \n");
				hexdump((u8 *) pbuf, psize);
				
				ret = do_ioctl(pbuf, psize);
				pbuf->request = CMD_QUERY_INFORMATION_RESPONSE;

				/* Return success */
				if (ret == 0) {
					printf("CMD_QUERY_INFORMATION success\n");
                    printf("SendBak bytes:%d\n",pbuf->u.info.length);
					sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN + pbuf->u.info.length, 0, (struct sockaddr *) &cli, cli_len);
					hexdump((u8 *) pbuf, ZD_GENERIC_OID_HDR_LEN + pbuf->u.info.length);
				}
				else {
					printf("CMD_QUERY_INFORMATION fail\n");
					//sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN, 0, (struct sockaddr *) &cli, cli_len);
					hexdump((u8 *) pbuf, ZD_GENERIC_OID_HDR_LEN);
				}

				break;

			case CMD_SET_INFORMATION:
				printf("CMD_SET_INFORMATION: \n");
				hexdump((u8 *) pbuf, psize);

				ret = do_ioctl(pbuf, psize);
				pbuf->request = CMD_SET_INFORMATINO_RESPONSE;

				/* Return success */
				if (ret == 0) {
					printf("CMD_SET_INFORMATION success\n");
                    sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN + pbuf->u.info.length, 0, (struct sockaddr *) &cli, cli_len);
				}
				else {
					printf("CMD_SET_INFORMATION fail\n");
				}

				//sendto(msock, pbuf, ZD_GENERIC_OID_HDR_LEN, 0, (struct sockaddr *) &cli, cli_len);
				//hexdump((u8 *) pbuf, ZD_GENERIC_OID_HDR_LEN);
				break;

			default:
				printf("command: 0x%04x not support\n", pbuf->request);
				break;
			}

		}

	}

	return 0;
}