自定义以太网帧格式组包

//data_len&0x01 to make sure the data length is even
#define util_add_field_to_frame(cmd,data,data_len)     do\
                                                    {\
                                                        unsigned char temp_value=cmd;\
                                                        util_add_data_to_frame(&temp_value,LENGTH_OF_FIELD);\
                                                        temp_value=data_len;\
                                                        util_add_data_to_frame(&temp_value,LENGTH_OF_FIELD_LENGTH);\
                                                        if( ( data ) != NULL )\
                                                        {\
                                                            util_add_data_to_frame(data,data_len);\
                                                        }\
                                                        if( ( data_len )&0x01 )\
                                                        {\
                                                            temp_value=0;\
                                                            util_add_data_to_frame(&temp_value,1);\
                                                        }\
                                                    }while(0)

#define util_add_data_to_frame(data_ptr,size)    do\
                                                    {\
                                                        if( real_frame_size + size > ETH_FRAME_LEN-20 )\
                                                        {\
                                                            return -1;\
                                                        }\
                                                        memcpy(p_ethernet_frame,data_ptr,size);\
                                                        real_frame_size += size;    \
                                                        p_ethernet_frame += size;    \
                                                    }while(0)

// below is marco and functions for recv
#define util_get_common_data_from_frame(data_ptr,len)    do\
                                                        {\
                                                            if(current_len + len > frame_len)\
                                                            {\
                                                                return -1;\
                                                            }\
                                                            if( ( data_ptr ) && ( len ) )\
                                                            {\
                                                                memcpy(data_ptr,p_ethernet_frame, len);\
                                                            }\
                                                            p_ethernet_frame += len;\
                                                            current_len += len;\
                                                        }while(0)

自定义协议函数填充以及提取宏。

util_add_field_to_frame(CMD_TYPE_PROBE_SERVER,NULL,0);

 

#if 1//ndef ETH_FRAME_LEN
#define ETH_FRAME_LEN 1514
#endif

#if 1//ndef ETH_ALEN
#define ETH_ALEN 6
#endif

#define LENGTH_OF_ETHERNET_DST_MAC 6
#define LENGTH_OF_ETHERNET_SRC_MAC 6
#define LENGTH_OF_ETHERNET_TYPE 2
#define LENGTH_OF_ETHERNET_LEN 2
#define LENGTH_OF_ETHERNET_CHECKSUM 2
#define LENGTH_OF_FIELD 1
#define LENGTH_OF_FIELD_LENGTH 1

#define OFFSET_OF_ETHERNET_DST_MAC 0
#define OFFSET_OF_ETHERNET_SRC_MAC ( OFFSET_OF_ETHERNET_DST_MAC+LENGTH_OF_ETHERNET_DST_MAC )
#define OFFSET_OF_ETHERNET_TYPE ( OFFSET_OF_ETHERNET_SRC_MAC+LENGTH_OF_ETHERNET_SRC_MAC )
#define OFFSET_OF_ETHERNET_LEN ( OFFSET_OF_ETHERNET_TYPE+LENGTH_OF_ETHERNET_TYPE )
#define OFFSET_OF_ETHERNET_DATA ( OFFSET_OF_ETHERNET_LEN+LENGTH_OF_ETHERNET_LEN )

/*Send frame function*/
int util_send_ethernet_frame(unsigned short ethernet_frame_type
                            ,unsigned char *ethernet_data
                            ,int ethernet_data_len
                            ,unsigned char* src_mac
                            ,unsigned char* dst_mac
                            )
{
    unsigned short b16;
    //int i;
    int result;
    struct ifnet * txifp;
    int real_frame_size;
    unsigned char *p_ethernet_frame;
    unsigned char ethernet_frame[ETH_FRAME_LEN];

    txifp = ifunit("apcli0");
    p_ethernet_frame = ethernet_frame;

    real_frame_size = 0;
    p_ethernet_frame = ethernet_frame;

    //destination mac
    util_add_data_to_frame(dst_mac,ETH_ALEN);
    //source mac
    util_add_data_to_frame(src_mac,ETH_ALEN);
    //data type in the ethernet frame
    b16 = htons( ethernet_frame_type );
    util_add_data_to_frame(&b16,sizeof(b16));

    p_ethernet_frame += LENGTH_OF_ETHERNET_LEN;// skip len

    //copy ethernet data
    memcpy(p_ethernet_frame,ethernet_data,ethernet_data_len);
    if( ethernet_data_len&0x01 )
    {
        p_ethernet_frame[ ethernet_data_len ]=0;
        ethernet_data_len++;
    }

    // dst_mac[6]. src_mac[6]. eth[2]
    p_ethernet_frame = ethernet_frame+OFFSET_OF_ETHERNET_LEN;
    // 2 bytes checksum
    b16 = htons( ethernet_data_len+LENGTH_OF_ETHERNET_CHECKSUM );
    memcpy(p_ethernet_frame,&b16,sizeof(b16));

    real_frame_size=OFFSET_OF_ETHERNET_DATA+ethernet_data_len+LENGTH_OF_ETHERNET_CHECKSUM;

    //calculate the checksum
    b16=ip_checksum(ethernet_frame,real_frame_size-LENGTH_OF_ETHERNET_CHECKSUM);
    memcpy(ethernet_frame+real_frame_size-LENGTH_OF_ETHERNET_CHECKSUM,&b16,sizeof(b16));

    //above all for p11_linux check_num
    //for ecos real pac size except dst_mac[6]. src_mac[6]. eth[2]
    real_frame_size=ethernet_data_len+LENGTH_OF_ETHERNET_LEN+LENGTH_OF_ETHERNET_CHECKSUM;
    //diag_printf("real_frame_size:%d checksum:%x\n",real_frame_size, b16);
    result = TZ_LIB_etherpacket_send((struct ifnet *)txifp, NULL, dst_mac, ethernet_frame_type, (char *)ethernet_frame+OFFSET_OF_ETHERNET_LEN, real_frame_size);

    if (result <= 0) 
    {
        diag_printf("util_send_ethernet_frame failed\n");
    }

    return 0;
}

int util_client_send_frame(void)
{
    //resend search server message
    int real_frame_size = 0;
    unsigned char *p_ethernet_frame = frame_send_buffer;
    //add  cmd
    util_add_field_to_frame(CMD_TYPE_AP_WIFI_ACK,NULL,0);

    //send ethernet frame
    util_send_ethernet_frame(ETHERNET_FRAME_TYPE_TO_SERVER
                                ,frame_send_buffer
                                ,real_frame_size
                                ,src_mac_addr
                                ,dst_mac_addr
                                );
    return 0;
}

static unsigned short ip_checksum(const void *data, int len)
{
    unsigned short checksum = 0;
    int left = len;
    const unsigned char *data_8 = data;

    while(left > 1) {
        checksum += ( ( *data_8 )<<8 )+( *(data_8+1) );
        data_8 += 2;
        left -= 2;
    }

    if(left) {
        checksum += ( ( *( unsigned char* )data_8 )<<8 );
    }

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

    checksum = (~checksum) & 0xffff;

    return htons(checksum);
}

 

转载于:https://www.cnblogs.com/fogcell/p/8378438.html