345 lines
9.4 KiB
C++
345 lines
9.4 KiB
C++
/*****************************************************************************
|
||
* @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 "eth0"
|
||
|
||
#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;
|
||
}
|