rf_usb_mac.c

VC下USB的驱动代码,及其相关操作检测。

#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<RF_MAC_FRM_HEAD/sizeof(short); k+=4) 
    {
        _rf_emac_scram(&frm[k]);
        _encrypt((unsigned short *)&frm[k], crypt_key);
    }
}

/* descramble frame - used by host
*
*   decrypt a report frame(meter to host)
*
*   CRC is also scrambled, to discourage attack 
*/
int rf_emac_descram(unsigned short * crypt_key, unsigned short * frm)
{
    int k;

    /* For meter reported frame the key is here
    */
    _decrypt((unsigned short *)&frm[0], crypt_key);
    _memcpy((unsigned char *)&_g_rf_key[0], (unsigned char *)frm, sizeof(_g_rf_key));
        
    /*_decrypt
    */
    for(k=4; k<RF_MAC_FRM_HEAD/sizeof(short); k+=4) 
    {
        _decrypt((unsigned short *)&frm[k], crypt_key);
        _rf_emac_scram(&frm[k]);
    }

    return 0;
}

unsigned char blank_deviceID[] = {0xFF,0xFF,0xFF,0xFF,0xFF,0xFF};

unsigned char blank_frame[RF_MAC_FRM_LEN];

unsigned char assign_deviceID[] = {0x11,0x22,0x33,0x44,0x55,0x66};

void _device_ID_inc(unsigned char * deviceID)
{
    deviceID[5] ++;
    if(deviceID[5] != 0)
    {
        return;
    }
    deviceID[4] ++;
    if(deviceID[4] != 0)
    {
        return;
    }
    deviceID[3] ++;
    if(deviceID[3] != 0)
    {
        return;
    }
    deviceID[3] ++;
    if(deviceID[3] != 0)
    {
        return;
    }
    deviceID[2] ++;
    if(deviceID[2] != 0)
    {
        return;
    }
    deviceID[1] ++;
    if(deviceID[1] != 0)
    {
        return;
    }
    deviceID[0] ++;
    if(deviceID[0] != 0)
    {
        return;
    }
    
}

int _RF_USB_send_frame(unsigned char * frame, unsigned int flen)
{
    rf_emac_crc(frame);

    PrintTime("\n-> 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;
}