PracticeDev/study_clang/Mimic/time_sync/frame_incrypt.cpp

/*****************************************************************************
 * @Author                      : Songsk
 * @Date                        : 2020-07-17 00:02:36
 * @LastEditors                 : songshuaikang@comleader.com.cn
 * @LastEditTime                : 2020-07-20 23:17:04
 * @Description                 : file content
 *******************************************************************************/

#include <stdio.h>
#include <iostream>
#include <stdlib.h>
#include <time.h>
#include <string.h>
#include <unistd.h>
#include <assert.h>
#include <pcap.h>

#define ACTIVE_NUM_NUM      3
#define UNSIGNED_INT_LEN    32
#define MAX_PADDING_SIZE   128
#define uint unsigned int
#define uchar unsigned char

#define _ENCRYPT(a,b,c) (a=(b^c)) 
#define _DECRYPT _ENCRYPT
#define _CHECKSUM(_checksum, _cipher, _key, _len)\
        {\
            uint _m_i = 0;\
            uint sum = 0;\
            for(_m_i=0; _m_i< _len; _m_i++)\
                sum = sum + (0xffffffff & _cipher[_m_i]) + (0xffffffff & _key[_m_i]);\
            _checksum =  0xff - (0xff & (sum)) - ((sum) >> 8);\
        }
#define ENCRYPT_FUNC(_cipher, _plain, _key, _len)\
{\
    int _m_i = 0;\
    for(_m_i=0; _m_i< _len; _m_i++)\
        _ENCRYPT(_cipher[_m_i], _plain[_m_i], _key[_m_i]);\
}

#define DECRYPT_FUNC(_plain, _cipher,  _key, _len)\
{\
    int _m_i = 0;\
    for(_m_i=0; _m_i< _len; _m_i++)\
        _DECRYPT( _plain[_m_i],_cipher[_m_i], _key[_m_i]);\
}

static unsigned int seed = 0;
#define SNAP_LEN 65536
#define INTERFACE_STR   "eno3"

#define SHD_PRIVATE_PROTOCOL_H  0x09

#define HexPrint(_buf, _len) \
{\
    int _m_i = 0;\
    char *_m_buf = (char *)(_buf);\
    int _m_len = (int)(_len);\
    printf("[%s:%d] \r\n", __FUNCTION__, __LINE__);\
    printf("***************************************************\n");\
    for(_m_i = 0; _m_i < _m_len; _m_i++)\
    {\
        if(_m_i == 0) printf("\033[31m%02x \033[0m",_m_i);\
        printf("\033[32m%02x \033[0m", _m_buf[_m_i] & 0xff);\
        if(!((_m_i+1) % 16))  printf("\n\033[31m%02x \033[0m",_m_i);\
    }\
    printf("\nsize = %d\n***************************************************\n", _m_len);\
}

typedef struct frame_head_t
{
    uchar dmac[6];
    uchar smac[6];
    uchar dot1q[4];
    uchar protocol[2];
    uchar padding;
    uchar len;
}FRAME_HEAD_T;

typedef struct msg_to_proxy_t
{
    uchar master_nos = 1;
    uchar active_nos[3];
}MSG_TO_PROXY_T;

typedef struct timeval TS_TO_PROXY_T;

typedef enum shd_action_type_e
{
    ACTION_TELL_ACTIVE_NOS =1,
    ACTION_SEND_TIMESTAMP,
    ACTINON_MAX
}SHD_ACTION;

typedef enum shd_private_protocol_code_low_e
{
    PROTOCOL_TELL_ACTIVE_NOS = 0x88,
    PROTOCOL_SEND_TIMESTAMP,
    PROTOCOL_MAX
}SHD_PROTOCOL;

/**
 * @description: 发送数据包，发送的网卡在宏定义中指定
 * @param {pkt:数据包内容，包含包头与payload； len:数据包长度} 
 * @return: 0:success
 */
int send_packet_with_pcap(const uchar *pkt, uint len)
{
    static pcap_t *handle = NULL;
    char dev_str[64];
    char err_buf[PCAP_ERRBUF_SIZE];

    strcpy(dev_str, INTERFACE_STR);
    if(NULL == handle)
    {
        handle = pcap_open_live(dev_str, SNAP_LEN, 1, 1000, err_buf);
        if (handle == NULL)
        {
            fprintf(stderr, "Couldn't open device %s: %s\n", dev_str, err_buf);
            return 0;
        }
    }

    return pcap_sendpacket(handle, pkt, len);
}

/**
 * @description: 随机函数，原子操作，提供一个字节的随机值
 * @param void 
 * @return: unsigned char
 */
uchar _random(void)
{
    seed += (unsigned int)time(0) + 781;
    srand(seed);
    uchar temp = (uchar)(rand()%256);
    return temp;
}

uint fill_with_random(uchar* buf , uint len)
{
    for(int i = 0; i< len; i++)
        *(buf+i) =(_random());
    return len;
}

void print_buffer_hex(const char * text, uchar * buf, uint len)
{
    printf("%s\n", text);
    // for(int i = 0; i < len; i++)
    // {
    //     printf("%2X ",buf[i]);
    //     if(i != 0 && i % 16 == 0)
    //         printf("\n");
    // }
    HexPrint(buf, len);
    printf("\n");
}

int is_unicast_macaddr(uchar * mac)
{
    if(mac[0] & 1)
        return 1;
    return 0;
}

int make_frame_up(const int type, uchar *frame, uchar * ciphertext, uchar * key, uint len, uint frame_len)
{
    //int p = 0;
    
    /*fill buffer with random value*/
    fill_with_random(frame, frame_len);
    
    /*create a frame header, include Ehthernet header | vlan header | protocol code */
    FRAME_HEAD_T frame_head;
    do
    {
        printf(".");
        fill_with_random(frame_head.smac, sizeof(frame_head.smac));
    }
    while (is_unicast_macaddr(frame_head.smac));
    printf("\nfill smac\n");
    do
    {
        printf(".");
        fill_with_random(frame_head.dmac, sizeof(frame_head.dmac));
    }
    while (is_unicast_macaddr(frame_head.dmac));
    printf("\nfill dmac\n");
    frame_head.dot1q[0] = 0x81;
    frame_head.dot1q[1] = 0x00;
    frame_head.dot1q[2] = 0x07;
    frame_head.dot1q[3] = 0xd0;
    frame_head.protocol[0] = 0x08;
    //frame_head.protocol[1] = 0x06;
    (ACTION_TELL_ACTIVE_NOS == type) ? frame_head.protocol[1] = PROTOCOL_TELL_ACTIVE_NOS : frame_head.protocol[1] = PROTOCOL_SEND_TIMESTAMP;

    /*get a padding*/
    uchar padding = 0;
    while(1)
    {
        srand(time(NULL));
        int a = sizeof(FRAME_HEAD_T) + len * 2 + 1;
        printf("a is %d, frame_len is %d\n", a, frame_len);
        padding = (uchar)rand()%(frame_len
                            - sizeof(FRAME_HEAD_T)
                            - len * 2 - 1) 
                            + sizeof(FRAME_HEAD_T);
        if(padding + len * 2 > frame_len) 
            continue;
        frame_head.padding = padding;
        printf("padding is 0x%x\n", padding);
        frame_head.len = len;
        break;
    }
    
    /*copy frame header and payload into buffer*/
    memcpy(frame, &frame_head, sizeof(frame_head));
    memcpy(frame + padding, ciphertext, len);
    memcpy(frame + padding + len, key, len);
    char checksum=0;
    _CHECKSUM(checksum, ciphertext, key, len);
    print_buffer_hex("计算校验和：", (unsigned char *)&checksum,sizeof(checksum));
    memcpy(frame + padding + len + len, &checksum, 1);
    /*send packet finally*/
    print_buffer_hex("产生报文：", frame,frame_len);
    return send_packet_with_pcap(frame, frame_len);
}

int shd_send_cmd_to_proxy(MSG_TO_PROXY_T * msg)
{
    int ret = 0;
    uchar key[sizeof(MSG_TO_PROXY_T)];
    uchar ciphertext[sizeof(MSG_TO_PROXY_T)];
    
    /*get plaintext*/
    uchar plaintext[sizeof(MSG_TO_PROXY_T)];
    memcpy(plaintext, msg, sizeof(MSG_TO_PROXY_T));
    print_buffer_hex("产生明文：", plaintext, sizeof(plaintext));

    /*create a key with random num*/
    ret = fill_with_random(key, sizeof(key));
    print_buffer_hex("产生掩码：", key, sizeof(key));

    /*incrypt*/
    ENCRYPT_FUNC(ciphertext, plaintext, key,sizeof(MSG_TO_PROXY_T));
    print_buffer_hex("计算密文：", ciphertext, sizeof(ciphertext));
    /*create and send frame*/
    srand(seed);
    uint frame_len = sizeof(FRAME_HEAD_T) //以太网帧头
                    + sizeof(MSG_TO_PROXY_T) * 2 //信息量长度
                    + 1
                    + 10
                    + rand()%MAX_PADDING_SIZE;   //padding长度
    uchar * frame = (uchar*)malloc(frame_len);
    ret = make_frame_up(ACTION_TELL_ACTIVE_NOS ,frame, ciphertext, key, sizeof(key), frame_len);
    if(ret)
    {
        printf("send packet error\n");
        free(frame);
        return -1;
    }
    free(frame);

    return 0;
}

int shd_send_timestamp_to_nos()
{
    int ret = 0;
    uchar key[sizeof(TS_TO_PROXY_T)];
    uchar ciphertext[sizeof(TS_TO_PROXY_T)];
    uchar plaintext[sizeof(TS_TO_PROXY_T)];
    
    TS_TO_PROXY_T time;
    gettimeofday(&time,NULL); 

    memcpy(plaintext, &time, sizeof(TS_TO_PROXY_T));
    print_buffer_hex("产生明文：", plaintext, sizeof(plaintext));
    ret = fill_with_random(key, sizeof(key));
    print_buffer_hex("产生掩码：", key, sizeof(key));
    ENCRYPT_FUNC(ciphertext, plaintext, key,sizeof(TS_TO_PROXY_T));
    print_buffer_hex("计算密文：", ciphertext, sizeof(ciphertext));
    srand(seed);
    int a = rand();
    printf("seed is %x, rand is %d, MAX_PADDING_SIZE is %d\n", seed, a, MAX_PADDING_SIZE);
    uint frame_len = sizeof(FRAME_HEAD_T) //以太网帧头
                    + sizeof(TS_TO_PROXY_T) * 2 //信息量长度
                    + 1
                    + 10
                    + a % MAX_PADDING_SIZE;   //padding长度
    printf("frame_len is %u\n", frame_len);
    uchar * frame = (uchar*)malloc(frame_len);
    if(NULL == frame)
    {
        printf("malloc error!!\n");
    }else
    {
        printf("frame addr is %x\n", frame);
    }
    
    ret = make_frame_up(ACTION_SEND_TIMESTAMP,frame, ciphertext, key, sizeof(key), frame_len);
    if(ret)
    {
        printf("send packet error\n");
        free(frame);
        return -1;
    }
    free(frame);
    return 0;
}

int main()
{
    int ret = 0;
    while(1)
    {
        #if 0
        #else
        MSG_TO_PROXY_T msg;
        msg.master_nos = 1;
        msg.active_nos[0] = 1;
        msg.active_nos[1] = 2;
        msg.active_nos[2] = 3;

        // ret = shd_send_cmd_to_proxy(&msg);
        // if(ret)
        // {
        //     printf("cmd send error\n");
        //     return -1;
        // }
        ret = shd_send_timestamp_to_nos();
        if(ret)
        {
            printf("cmd send error\n");
            return -1;
        }
        #endif
        // usleep(5000);
        getchar();
    }
    
    return 0;
}