PracticeDev/study_clang/Mimic/time_sync/frame_incrypt.cpp

345 lines
9.4 KiB
C++
Raw Normal View History

2022-12-20 17:31:11 +08:00
/*****************************************************************************
* @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;
}