1 files changed, 443 insertions, 0 deletions

diff --git a/package/librtk-inband/src/inband_if.c b/package/librtk-inband/src/inband_if.c
new file mode 100644
index 000000000..a3ce40996
--- /dev/null
+++ b/package/librtk-inband/src/inband_if.c
@@ -0,0 +1,443 @@
+/*
+ * IOH daemon
+ * Copyright (C)2010, Realtek Semiconductor Corp. All rights reserved
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <signal.h>
+#include <unistd.h>
+#include <sys/ioctl.h>
+
+#include <linux/if_packet.h>
+#include "ioh.h"
+#include "inband_if.h"
+
+struct inband_header {
+	struct ioh_header raw_header;
+	unsigned char rrcp_type;	// should be RRCP_P_IOH
+	unsigned char inband_cmd;
+	unsigned short inband_seq;
+	unsigned short inband_frag;
+	unsigned char inband_reserved[2];
+	unsigned short inband_data_len;
+} __attribute__((packed));
+
+struct fragment_info{
+	unsigned char inband_cmd;
+	unsigned short inband_seq;
+	unsigned short inband_frag;
+	char *buf_ptr;
+	char *buf;	
+	unsigned int data_len;
+};
+
+struct inband_class {
+	struct ioh_class ioh_obj;
+	struct inband_header *tx_header;
+	struct inband_header *rx_header;
+	unsigned char *tx_data;
+	unsigned char *rx_data;
+	struct fragment_info frag_info;
+	unsigned int in_used;
+};
+
+//header : 4 sign , 4 opttion , 4 offset, 4 len
+#define FM_HEADER_LEN_OFFSET 12
+
+#define MAX_INBAND_CHAN 1
+#define MAX_PREALLOC_INBAND_CHAN MAX_INBAND_CHAN
+
+int inband_rcv_timeout=0; //mark_issue, not implement now
+
+static struct inband_class inband_obj[MAX_INBAND_CHAN];
+static int inband_ready=0;
+
+static void init_inband_obj(struct inband_class *ib_obj)
+{
+	struct ioh_class *obj=&ib_obj->ioh_obj;
+
+	ib_obj->tx_header = (struct inband_header *)obj->tx_header;
+	ib_obj->rx_header = (struct inband_header *)obj->rx_header;
+
+	ib_obj->tx_data = (unsigned char *) obj->tx_buffer + sizeof(struct inband_header);
+	ib_obj->rx_data = (unsigned char *) obj->rx_buffer + sizeof(struct inband_header);
+
+	//frag_info?
+}
+
+static unsigned int get_free_chan()
+{	
+	int i;
+	int chan=-1;
+	
+	for(i=0;i<MAX_INBAND_CHAN;i++)
+	{
+		if(inband_obj[i].in_used ==0 )			
+		{
+			inband_obj[i].in_used =1;
+			chan = i;
+			break;
+		}		
+	}
+	return chan;	
+}
+
+static struct inband_class *get_chan_obj(unsigned int chan)
+{
+	return (struct inband_class *)&inband_obj[chan];
+}
+
+static void inband_init_all()
+{
+	memset(&inband_obj[0],0,sizeof(struct inband_class)*MAX_INBAND_CHAN);
+}
+
+int inband_open(char *netif_name,char *slave_mac,unsigned short eth_type,int debug)
+{	
+	int ret;
+	unsigned int chan;
+	struct inband_class *inband_obj_p;
+	struct ioh_class *ioh_obj_p;
+
+	if(inband_ready == 0)
+	{
+		inband_init_all();
+		inband_ready =1 ;
+	}
+
+	chan = get_free_chan();
+	if(chan < 0)
+		return -1;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);
+
+	ioh_obj_p = &inband_obj_p->ioh_obj;	
+		
+	ret = ioh_open(ioh_obj_p, netif_name, slave_mac,eth_type, debug);
+
+	if(ret < 0 )
+		return -1;
+
+	init_inband_obj(inband_obj_p);
+
+	return chan;	
+}
+
+int get_inband_socket(int chan)
+{
+	struct inband_class *inband_obj_p;
+	struct ioh_class *ioh_obj_p;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);
+	ioh_obj_p = &inband_obj_p->ioh_obj;	
+	
+	return ioh_obj_p->sockfd;
+}
+
+int get_inband_destMac(int chan,char *destmac) //mark_test
+{
+	struct inband_class *inband_obj_p;
+	struct ioh_class *ioh_obj_p;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);
+	ioh_obj_p = &inband_obj_p->ioh_obj;	
+	
+	memcpy(destmac,ioh_obj_p->dest_mac,6);
+	
+	return 0;
+}
+
+void inband_close(int chan)
+{	
+	//clear frag_info
+	struct inband_class *inband_obj_p;
+	struct ioh_class *ioh_obj_p;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);	
+	ioh_obj_p = &inband_obj_p->ioh_obj;	
+	ioh_close(ioh_obj_p);	
+	inband_obj_p->in_used =0; //free the obj
+}
+
+static int inband_recv(struct inband_class *ib_obj,int timeout)
+{
+        struct ioh_class *ioh_obj_p = &ib_obj->ioh_obj;
+        int rx_len;
+
+        rx_len = ioh_recv(ioh_obj_p, timeout);
+        if (rx_len < 0)
+                        return ERROR_TIMOUT;
+        if(ib_obj->rx_header->rrcp_type != RRCP_P_IOH)
+                return -1;
+
+        return rx_len;
+}
+
+static int send_frag_ack(struct inband_class *ib_obj)
+{
+	struct ioh_class *ioh_obj_p = &ib_obj->ioh_obj;
+	
+	ib_obj->tx_header->rrcp_type = RRCP_P_IOH;  //mark_inband
+	ib_obj->tx_header->inband_cmd = ib_obj->rx_header->inband_cmd;
+	ib_obj->tx_header->inband_seq = ib_obj->rx_header->inband_seq;
+	ib_obj->tx_header->inband_frag = ib_obj->rx_header->inband_frag;
+	ib_obj->tx_header->inband_data_len = 0;
+	
+	return ioh_send(ioh_obj_p,sizeof(struct inband_header)); //only send header for ack
+}
+static int check_frag_ack(struct inband_class *ib_obj)
+{
+	int rx_len;	
+	
+	rx_len = inband_recv(ib_obj, PER_FRAME_TIMEOUT); // -1 = wait until rec
+	if (rx_len < 0)
+	{
+		perror("check_frag_ack fail:");
+		return 0;
+       }	
+	if((ib_obj->rx_header->inband_cmd == ib_obj->tx_header->inband_cmd) &&
+		(ib_obj->rx_header->inband_seq == ib_obj->tx_header->inband_seq) &&
+		(ib_obj->rx_header->inband_frag == ib_obj->tx_header->inband_frag) )
+		return 1; //ok
+
+	return 0; //fail
+}
+static char *inband_alloc_buf(struct inband_class *ib_obj)
+{
+     char *buf=NULL;
+     unsigned int buf_size=MAX_APP_DATA_SIZE;	 
+     unsigned char *image_len,header_len_offset=FM_HEADER_LEN_OFFSET;	 
+#if 1 //it's for firmware file 	 
+     if(ib_obj->rx_header->inband_cmd == id_firm_upgrade)
+     {
+     	   //read firmware length from it's header
+     	   image_len = (char *)ib_obj->rx_data;          
+          buf_size =(unsigned int)( ( image_len[header_len_offset+0] <<24 ) +
+						     ( image_len[header_len_offset+1] <<16 ) +
+						     ( image_len[header_len_offset+2] <<8 )   +
+						     ( image_len[header_len_offset+3] <<0 )   + 16 )	;						     	   
+
+	   //printf("inband_alloc_buf buf_size=%x , image_len[0]= %x\n",buf_size,image_len[header_len_offset+0]);	  
+     }    
+#endif
+    buf = (char *)malloc(buf_size);	 
+     return buf;	
+}
+//mark_issue,defragment_reset
+static int init_defragment_process(struct inband_class *ib_obj)
+{
+	struct fragment_info *p_frag_info = &ib_obj->frag_info;
+	
+	//check if the it is first fragment  id
+	if(ib_obj->rx_header->inband_frag != FIRST_FRAG_ID)
+		return ERROR_DEFRAGMENT;
+
+	if((p_frag_info->buf = (char *)inband_alloc_buf(ib_obj)) == NULL)
+  	{
+		printf("init_defragment_process : data buffer allocation failed!\n");
+		return ERROR_DEFRAGMENT;
+	}
+	p_frag_info->buf_ptr = p_frag_info->buf;
+	p_frag_info->inband_frag = FIRST_FRAG_ID;
+
+#ifdef WPAS_INB
+	p_frag_info->inband_cmd = ntohs(ib_obj->rx_header->inband_cmd);
+	p_frag_info->inband_seq = ntohs(ib_obj->rx_header->inband_seq);
+#else
+	p_frag_info->inband_cmd = ib_obj->rx_header->inband_cmd;
+	p_frag_info->inband_seq = ib_obj->rx_header->inband_seq;
+#endif
+	
+	//copy first frame to buffer
+	memcpy(p_frag_info->buf,ib_obj->rx_data,ntohs(ib_obj->rx_header->inband_data_len));
+	p_frag_info->buf += ntohs(ib_obj->rx_header->inband_data_len);
+	p_frag_info->data_len = ntohs(ib_obj->rx_header->inband_data_len);
+	send_frag_ack(ib_obj);
+
+	return 0; //init ok
+}
+
+static int do_defragment_process(struct inband_class *ib_obj)
+{
+	int ret=0;
+	struct fragment_info *p_frag_info = &ib_obj->frag_info;
+	
+	if(p_frag_info->inband_seq !=  ntohs(ib_obj->rx_header->inband_seq) )
+		return ERROR_DEFRAGMENT;
+
+	if( (p_frag_info->inband_frag+1) !=  (ntohs(ib_obj->rx_header->inband_frag) & FRAG_ID_MASK))
+		return ERROR_DEFRAGMENT;
+	else
+		p_frag_info->inband_frag = ntohs(ib_obj->rx_header->inband_frag);
+
+	memcpy(p_frag_info->buf,ib_obj->rx_data,ntohs(ib_obj->rx_header->inband_data_len));
+	p_frag_info->buf += (ntohs(ib_obj->rx_header->inband_data_len));
+	p_frag_info->data_len+= (ntohs(ib_obj->rx_header->inband_data_len));
+
+	if( (p_frag_info->inband_frag & EOF_BIT) == EOF_BIT)
+		ret = 1;  
+	
+	return ret; // return 1 means EOF rcv , return 0 means conitiune
+}
+
+static int get_defragment_info(struct inband_class *ib_obj,char *cmd_type,char **data)
+{
+	struct fragment_info *p_frag_info = &ib_obj->frag_info;
+	
+	*cmd_type = p_frag_info->inband_cmd;
+	*data = p_frag_info->buf_ptr; //mark_issue,hwo to free the buffer ?
+	return p_frag_info->data_len;
+}
+
+static int inband_rcv_fragment(struct inband_class *ib_obj,char *cmd_type,char **data)
+{
+	int rx_len,ret=0;		
+
+	ret = init_defragment_process(ib_obj) ;
+
+	if(ret < 0)
+		return ret;	
+	
+	while( (inband_rcv_timeout!=1))
+	{
+		rx_len = inband_recv(ib_obj, PER_FRAME_TIMEOUT);
+		if (rx_len < 0)
+			return ERROR_TIMOUT;
+		ret = do_defragment_process(ib_obj);
+		if (ret < 0)
+			return ret;
+		else if(ret == 1) //ret == 1 means defragment end , ret=0 means contiune			
+		{
+			ret = get_defragment_info(ib_obj,cmd_type,data);
+			break;
+		}	
+		send_frag_ack(ib_obj);
+	}		
+	return ret;
+}
+void inband_free_buf(char *data_buf,int data_len)
+{
+	//only free allocated buffer from deframet process.
+	if( (data_buf != NULL) && ( data_len > MAX_INBAND_PAYLOAD_LEN ) )
+		free(data_buf);
+}
+int inband_rcv_data(int chan,char *cmd_type,char **data,int timout_ms) //return data length
+{	
+	int rx_len,data_len=0;
+	struct inband_class *inband_obj_p;
+	struct ioh_class *ioh_obj_p;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);	
+	ioh_obj_p = &inband_obj_p->ioh_obj;	
+
+	
+	//timout_ms will be used only for the first pkt. if the pkt is fragmented then every packet will
+	//follow fragment_timout_ms
+	rx_len = inband_recv(inband_obj_p, timout_ms); // -1 = wait until rec
+	if (rx_len < 0)
+	{
+		perror("inband_rcv_data:");
+		return -1;
+       }
+	//printf("inband_rcv_data:\n");
+	//hex_dump(ioh_obj_p->rx_buffer, ntohs(inband_obj_p->rx_header->inband_data_len) + sizeof(*inband_obj_p->rx_header)); //mark_test		
+
+	//cache for tx dest mac
+	if( memcmp(ioh_obj_p->dest_mac,ioh_obj_p->rx_header->sa,6)) //mark_test
+		memcpy(ioh_obj_p->dest_mac,ioh_obj_p->rx_header->sa,6);
+
+	//single pkt	
+	if( inband_obj_p->rx_header->inband_frag  == ntohs(SINGLE_FRAME)) //mark_endian
+	{		
+		*cmd_type = inband_obj_p->rx_header->inband_cmd;
+		data_len =  ntohs(inband_obj_p->rx_header->inband_data_len);		
+		*data = inband_obj_p->rx_data ; //or memcpy;	
+	}
+	else //fragment process
+		data_len = inband_rcv_fragment(inband_obj_p,cmd_type,data);
+
+	return data_len;
+	
+}
+
+//if seq is need in your application
+int inband_rcv_data_and_seq(int chan,unsigned int *seq,char *cmd_type,char **data,int timout_ms) //return data length
+{
+	struct inband_class *inband_obj_p;
+	int ret=0;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);	
+	ret = inband_rcv_data(chan,cmd_type,data, timout_ms); //return data length
+	if(ret < 0)
+		return -1;
+
+	*seq = ntohs(inband_obj_p->rx_header->inband_seq);
+	return ret;
+}
+
+static int inband_send_data(struct inband_class *ib_obj,char *data,int data_len)
+{	
+	char *frag_ptr;
+	unsigned short id=0,total_frag=0;
+	unsigned int last_num;
+	struct ioh_class *ioh_obj_p = &ib_obj->ioh_obj;
+
+	total_frag = (unsigned short)(data_len / MAX_INBAND_PAYLOAD_LEN);
+		
+	if( total_frag > MAX_FRAG_ID)
+		return -1;
+
+	ib_obj->tx_header->inband_frag =0;
+	frag_ptr = data;
+
+	for(id=0;id<total_frag;id++)
+	{
+		ib_obj->tx_header->inband_data_len = htons(MAX_INBAND_PAYLOAD_LEN);
+		ib_obj->tx_header->inband_frag = htons(id );
+		memcpy(&ib_obj->tx_data[0], frag_ptr,MAX_INBAND_PAYLOAD_LEN );		
+		if( ioh_send(ioh_obj_p, sizeof(struct inband_header) + MAX_INBAND_PAYLOAD_LEN ) < 0)
+			return -1;
+		//if(id>= 1){
+		if(check_frag_ack(ib_obj) != 1)
+			return -1;			
+		//}	
+		frag_ptr += MAX_INBAND_PAYLOAD_LEN;
+	}
+	last_num = data_len % MAX_INBAND_PAYLOAD_LEN;
+	//EOF fragment	
+	ib_obj->tx_header->inband_frag = id;
+	ib_obj->tx_header->inband_frag |=EOF_BIT;
+	ib_obj->tx_header->inband_frag = htons(ib_obj->tx_header->inband_frag);
+	ib_obj->tx_header->inband_data_len = htons(last_num);
+	if(last_num >0)
+		memcpy(&ib_obj->tx_data[0], frag_ptr,last_num );
+
+	return ioh_send(ioh_obj_p,sizeof(struct inband_header)+last_num);		
+}
+
+int inband_write(int chan,unsigned int seq,char cmd,char *data,int data_len,int reply)
+{
+	struct inband_class *inband_obj_p;
+
+	inband_obj_p = (struct inband_class *)get_chan_obj(chan);	
+
+	inband_obj_p->tx_header->rrcp_type = RRCP_P_IOH;  //mark_inband
+	//fill inband header , cmd
+	inband_obj_p->tx_header->inband_cmd = cmd;
+
+	//reply = 0(request) ,reply = 1(good reply),reply = 2(bad reply)
+	if(reply == 2)
+		inband_obj_p->tx_header->inband_cmd |= CMD_ERROR_REPLY_BIT;
+
+	//fill inband header , seq
+	if(!reply) 
+		inband_obj_p->tx_header->inband_seq = htons(seq);
+	else //seq is not used when the packet is for reply
+		inband_obj_p->tx_header->inband_seq = inband_obj_p->rx_header->inband_seq;	
+	
+	//fill data, data_len , and send
+	return inband_send_data(inband_obj_p,data,data_len);
+}
+