www.pudn.com > Usb_Project.rar > RF_USB_MAC.c
#include "RF_USB_MAC.h"
typedef int (* RF_USB_RTS_FUNC)(void);
extern RF_USB_RTS_FUNC RF_USB_RTS;
extern unsigned short crc16(unsigned char* data, unsigned short len);
int RF_USB_send_frame(unsigned char * frame, unsigned int flen);
static unsigned char RF_send_index = 0;
extern void *memset(void *s, int c, unsigned int n);
int RF_USB_ready_to_send(void)
{
unsigned char frm[RF_MAC_FRM_LEN];
struct rf_mac_hdr * _hptr = RF_MAC_HDR(frm);
memset(frm, 0, sizeof(frm));
_hptr->idx_len = RF_send_index++;
RF_USB_send_frame(frm, RF_MAC_FRM_DATA_LEN);
return 0;
}
int RF_USB_MAC_Init(void)
{
RF_USB_RTS = RF_USB_ready_to_send;
RF_USB_ready_to_send();
return 0;
}
//key for scramble
unsigned short _g_rf_key[4];
void _rf_emac_scram(unsigned short * data)
{
data[0] ^= _g_rf_key[0];
data[1] ^= _g_rf_key[1];
data[2] ^= _g_rf_key[2];
data[3] ^= _g_rf_key[3];
}
/* memory copy byte by byte
*/
void _memcpy(unsigned char * dst, const unsigned char * src, unsigned short len)
{
while(len -- > 0)
{
*(dst ++) = *(src ++);
}
}
/* memory compare byte by byte
*/
int _memcmp(const unsigned char * dst, const unsigned char * src, unsigned short len)
{
while(len -- > 0)
{
if(*(dst ++) != *(src ++)) {
return 1;
}
}
return 0;
}
//key is provider/customer specific
const unsigned short cryption_key[] = {0x0312, 0x1225, 0xF413, 0x4300};
//key is provider/customer specific
const unsigned short decrypt_key[] = {0x5460, 0x4510, 0x7550, 0x1021};
static void _swap(unsigned short *key)
{
unsigned short key2;
key2 = key[1];
key[1] = (key[0] & 0xFF) | (key[1] & 0xFF00);
key[0] = (key2 & 0xFF) | (key[0] & 0xFF00);
}
static void _xor(unsigned short *key, unsigned short * crypt_key)
{
key[0] ^= crypt_key[0];
key[1] ^= crypt_key[1];
}
/*
* _decrypt
*/
static void __decrypt(unsigned short *key, unsigned short *crypt_key)
{
_xor(key, crypt_key);
_swap(key);
key[0] = (key[0]<<11) |(key[0]>>5);
key[1] = ((key[1]>>9)&0x1F) | ((key[1]<<5)&0x3FE0) | (key[1]&0xC000);
}
void _decrypt(unsigned short *key, unsigned short *crypt_key)
{
__decrypt(key, crypt_key);
__decrypt(&key[2], &crypt_key[2]);
}
/*
* _encrypt
*/
static void __encrypt(unsigned short *key, unsigned short *crypt_key)
{
key[0] = (key[0]<<5) |(key[0]>>11);
key[1] = ((key[1]>>5)&0x1FF) | ((key[1]<<9)&0x3E00) | (key[1]&0xC000);
_swap(key);
_xor(key, crypt_key);
}
void _encrypt(unsigned short *key, unsigned short *crypt_key)
{
__encrypt(key, crypt_key);
__encrypt(&key[2], &crypt_key[2]);
}
/* CRC of frame
*
* do CRC before scramble, to discourage attack
*/
int rf_emac_crc(unsigned char * em_data)
{
unsigned short crc_k;
unsigned short crc_f;
//CRC
crc_k = crc16(em_data, RF_MAC_FRM_HEAD-2);
crc_f = (em_data[RF_MAC_FRM_HEAD-1]<<8) | (em_data[RF_MAC_FRM_HEAD-2]);
em_data[RF_MAC_FRM_HEAD-2] = (unsigned char)crc_k;
em_data[RF_MAC_FRM_HEAD-1] = crc_k>>8;
if(crc_k != crc_f)
{
/*CRC error*/
return 1;
} else {
/*CRC check OK*/
return 0;
}
}
/* enscramble frame - used by host
*
* encrypt a config frame(host to meter)
*
* CRC is also scrambled, to discourage attack
*/
void rf_emac_enscram(unsigned short * crypt_key, unsigned short * frm)
{
int k;
//do CRC
// rf_emac_crc();
/*encrypt, leave aside the first 8bytes,
* and last 2 bytes for CRC
*/
for(k=0; k Decoded TX Time:");
PrintHexBuffer((unsigned char *)&frame[0], flen);
LogTime("\n-> Decoded TX Time:");
LogHexBuffer((unsigned char *)&frame[0], flen);
rf_emac_enscram((unsigned short *)&decrypt_key[0], (unsigned short *)frame);
RF_USB_send_frame(frame, flen);
return 0;
}
static void RF_preset_config_frame(unsigned char * frame)
{
struct rf_cwm_cfg_hdr * _cptr = (struct rf_cwm_cfg_hdr *)&frame[0];
memset((unsigned char *)&frame[0], 0, RF_MAC_FRM_DATA_LEN);
_cptr->meter_type = MTYPE_COLDWATER;
_cptr->proto_ver = PROTO_CONFIRM_VER1;
_cptr->data.flag = 0x55AA;
}
unsigned int selected_mode = 0;
int _RF_USB_recv_frame_cmd0(unsigned char * frame, unsigned int flen)
{
struct rf_mac_hdr * _hptr;
_hptr = (struct rf_mac_hdr *)&frame[RF_MAC_FRM_HEAD];
//from rfd
if(_hptr->cmd == 0)
{
struct rf_cwm_rpt_hdr * _rptr;
struct rf_cwm_cfg_hdr * _cptr;
_rptr = (struct rf_cwm_rpt_hdr *)&frame[0];
_cptr = (struct rf_cwm_cfg_hdr *)&blank_frame[0];
rf_emac_descram((unsigned short *)&cryption_key[0], (unsigned short *)frame);
//for CRC16
memset((unsigned char *)&frame[0], 0, sizeof(_g_rf_key));
if(rf_emac_crc(frame) != 0)
{
PrintTime("\n<- RX CRC ERROR Time:");
LogTime("\n<- RX CRC ERROR Time:");
return -1;
}
//put key back
_memcpy(frame, (unsigned char *)&_g_rf_key[0], sizeof(_g_rf_key));
PrintTime("\n<- Decoded RX Time:");
PrintHexBuffer((unsigned char *)&frame[0], flen);
LogTime("\n<- Decoded RX Time:");
LogHexBuffer((unsigned char *)&frame[0], flen);
if(_memcmp(_rptr->data.id, blank_deviceID, sizeof(blank_deviceID)) == 0)
{
if(selected_mode == 1)
{
RF_preset_config_frame(blank_frame);
_memcpy(_cptr->data.id, assign_deviceID, sizeof(assign_deviceID));
_RF_USB_send_frame(blank_frame, RF_MAC_FRM_DATA_LEN);
}
}
else
{
if(_memcmp(_rptr->data.id, assign_deviceID, sizeof(assign_deviceID)) == 0)
{
_device_ID_inc(assign_deviceID);
}
if(selected_mode == 2)
{
if(_rptr->data.mver == 0xFFFF)
{
RF_preset_config_frame(blank_frame);
_memcpy(_cptr->data.id, _rptr->data.id, sizeof(assign_deviceID));
_cptr->data.cmd[0] = 0x80;
_cptr->data.cmd[1] = 0x80;
_RF_USB_send_frame(blank_frame, RF_MAC_FRM_DATA_LEN);
}
}
else if(selected_mode == 3)
{
if(_rptr->data.stored_exp)
{
RF_preset_config_frame(blank_frame);
_memcpy(_cptr->data.id, _rptr->data.id, sizeof(assign_deviceID));
_RF_USB_send_frame(blank_frame, RF_MAC_FRM_DATA_LEN);
}
}
else if(selected_mode == 4)
{
RF_preset_config_frame(blank_frame);
_RF_USB_send_frame(blank_frame, RF_MAC_FRM_DATA_LEN);
}
else if(selected_mode == 5)
{
RF_preset_config_frame(blank_frame);
_memcpy(_cptr->data.id, _rptr->data.id, sizeof(assign_deviceID));
_cptr->data.cmd[0] = 0xFF;
_cptr->data.cmd[1] = 0x01;
//zeroing flag
_memcpy(_cptr->data.memo, _rptr->data.id, sizeof(assign_deviceID));
_RF_USB_send_frame(blank_frame, RF_MAC_FRM_DATA_LEN);
}
}
}
return 0;
}
extern int _RF_USB_recv_frame_cmd11(unsigned char * frame, unsigned int flen);
int RF_USB_recv_frame(unsigned char * frame, unsigned int flen)
{
struct rf_mac_hdr * _hptr;
_hptr = (struct rf_mac_hdr *)&frame[RF_MAC_FRM_HEAD];
//from rfd
if(_hptr->cmd == 0)
{
_RF_USB_recv_frame_cmd0(frame, flen);
}
else if((_hptr->cmd >= 0x11) && (_hptr->cmd <= 0x1F))
{
_RF_USB_recv_frame_cmd11(frame, flen);
}
else
{
}
return 0;
}