test_prj/ble_simple_peripheral/code/aicare_handler.c

#include "aicare_handler.h"
#include "aicare_config.h"
#include "os_mem.h"

#define AICARE_LOG	printf
/*
vid: 0x005E 

uint32_t key[4] = {0xEE7F92D2, 0x446A4386, 0xBE4319B1, 0x34BC10B9} 

握手指令 payload 示例(演示固定值): 
App 下发明文:  
HEX: <00010203 04050607 08090A0B 0C0D0E0F> 
下位机 反馈密文: 
HEX: <81BEB013 AEEEFFB0 86494180 7D9A84E1> 

握手指令 payload 示例(随机值): 
App 下发明文:  
HEX: <84536762 D5B882EA 117BFE43 A49A0407> 
下位机 反馈密文: 
HEX: <AA206138 585F7E63 31299D3C 6FBE658F> 
*/

static uint32_t aicare_key[4] = { 0xEE7F92D2, 0x446A4386, 0xBE4319B1, 0x34BC10B9 };

//## 加密算法
static void encrypt_tea(uint32_t *v, uint32_t *key) {
    uint32_t v0 = v[0], v1 = v[1], sum = 0, i=0;
    uint32_t delta = 0x9e3779b9;
    uint32_t k0 = key[0], k1 = key[1], k2 = key[2], k3 = key[3];
    for (i = 0; i < 32; i++) {
        sum += delta;
        uint32_t a=(v1 << 4);
        uint32_t b=(v1 >> 5);
        v0 += (a + k0) ^ (v1 + sum) ^ (b + k1);
        uint32_t c=(v0 << 4);
        uint32_t d=(v0 >> 5);
        v1 += (c + k2) ^ (v0 + sum) ^ (d + k3);
    }
    v[0] = v0;
    v[1] = v1;
}

//16byte加密
static void encrypt(uint8_t *p,uint32_t *key) {
    union u8_to_u32 {
        uint8_t u8_data[16];
        uint32_t u32_data[4];
    }v={0};
	 
    memcpy((uint8_t *) &v.u8_data, p, sizeof(v));

    encrypt_tea(&v.u32_data[0],  key);
    encrypt_tea(&v.u32_data[2],  key);

    memcpy(p, (uint8_t *) &v.u8_data, sizeof(v));
}

static uint16_t aicare_sum_check(uint8_t *pbuff, uint16_t len)
{
    if((len==0) || (pbuff==NULL))
		return 0;
    uint16_t temp = 0;
    for(uint16_t j = 0; j < len; j++)
    {
        temp += pbuff[j];
    }
    return temp;
}
void aicare_app_data_decode(uint8_t protocol_flag,uint8_t frame_num,uint8_t frame_sum,uint16_t data_len,uint8_t *buf);
void aicare_app_data_encode(uint8_t protocol_flag,uint8_t frame_num,uint8_t frame_sum,uint16_t data_len,uint8_t *buf);
void aicare_app_ack_encode(uint8_t protocol_flag,uint8_t frame_num,uint8_t frame_sum,uint16_t data_len,uint8_t *buf,uint8_t rsp);

void aicare_app_data_recv(uint8_t *data, uint16_t len)
{
	  uint16_t src_len = ((data[6]<<8)|data[7]);
	  uint16_t src_check = ((data[4]<<8)|data[5]);
		if((data[0]==0xdd) && ((src_len+8)==len))
		{
				uint16_t check_val=aicare_sum_check(&data[8],src_len);
			  //AICARE_LOG("check_val:%x\r\n",check_val);
			  if(src_check == check_val)
				{
					aicare_app_data_decode(data[1],data[2],data[3],src_len,&data[8]);
				}
		}

}

void aicare_app_data_decode(uint8_t protocol_flag,uint8_t frame_num,uint8_t frame_sum,uint16_t data_len,uint8_t *buf)
{
		aicare_tlv_t tlv;
		tlv.type = (buf[0]<<8)|buf[1];
	  tlv.status = buf[2];
	  tlv.length = (buf[3]<<8)|buf[4];
	  tlv.data = &buf[5];
		AICARE_LOG("protocol_flag:%x frame_num:%d frame_sum:%d data_len:%d\r\n",protocol_flag,frame_num,frame_sum,data_len);
	  switch(tlv.type)
		{
			case AICARE_HANDSHAKE1_CMD:
					 if((tlv.length==0x0015) && (protocol_flag==0x01))
					 {
							encrypt(tlv.data,aicare_key);	//加密
							tlv.length +=1;
							buf[3] = tlv.length>>8;
							buf[4] = tlv.length;
							aicare_app_ack_encode(protocol_flag,frame_num,frame_sum,data_len,&buf[0],0x00);
					 }
					 else if((tlv.length==0x0005) && (protocol_flag==0x11)) //app ack
					 {
							  //rsp handshake2 
						    
					 }
			break;
			
			case AICARE_HANDSHAKE2_CMD:
					 encrypt(tlv.data,aicare_key);	//加密
					 tlv.length +=1;
			     buf[3] = tlv.length>>8;
					 buf[4] = tlv.length;
					 aicare_app_ack_encode(protocol_flag,frame_num,frame_sum,data_len,&buf[0],0x00);
			break;
			
			
		}
		
}
 
void aicare_app_ack_encode(uint8_t protocol_flag,uint8_t frame_num,uint8_t frame_sum,uint16_t data_len,uint8_t *buf,uint8_t rsp)
{
	  if(buf==NULL)return;
	  uint16_t dst_len = data_len+8+1;
	  uint8_t *pack_buf=os_malloc(dst_len);
	  if(pack_buf==NULL)return;
	  pack_buf[0]=0xdd;
		pack_buf[1]=protocol_flag|0x10;//ack
	  pack_buf[2]=frame_num;
	  pack_buf[3]=frame_sum;
		
	  
	  data_len+=1;
	  pack_buf[6]=data_len>>8;
	  pack_buf[7]=data_len;
	
	  memcpy(&pack_buf[8],buf,data_len);
	  pack_buf[dst_len-1]=rsp;
	
	  uint16_t check = aicare_sum_check(&pack_buf[8],data_len);
	  pack_buf[4]=check>>8;
	  pack_buf[5]=check;
	
	  show_reg(pack_buf,dst_len,1);
		aicare_send_notify(pack_buf,dst_len);
	  
	  os_free(pack_buf);
	  pack_buf=NULL;
		AICARE_LOG("protocol_flag:%x frame_num:%d frame_sum:%d data_len:%d\r\n",protocol_flag,frame_num,frame_sum,data_len);
}

void aicare_app_data_encode(uint8_t protocol_flag,uint8_t frame_num,uint8_t frame_sum,uint16_t data_len,uint8_t *buf)
{
	  if(buf==NULL)return;
	  uint16_t dst_len = data_len+8;
	  uint8_t *pack_buf=os_malloc(dst_len);
	  if(pack_buf==NULL)return;
	  pack_buf[0]=0xdd;
		pack_buf[1]=protocol_flag;
	  pack_buf[2]=frame_num;
	  pack_buf[3]=frame_sum;
		
	  pack_buf[6]=data_len>>8;
	  pack_buf[7]=data_len;
	
	  memcpy(&pack_buf[8],buf,data_len);
	
	  uint16_t check = aicare_sum_check(&pack_buf[8],data_len);
	  pack_buf[4]=check>>8;
	  pack_buf[5]=check;
	  AICARE_LOG("encode data:\r\n");
	  show_reg(pack_buf,dst_len,1);
		aicare_send_notify(pack_buf,dst_len);
	  os_free(pack_buf);
	  pack_buf=NULL;
	
}