File content as of revision 11:d5edb6e3edab:

/*  
 Eric Tsai
 
 7/29/2017:  Added spoof checking using clock algorithm.  

*/

#include "mbed.h"
#include "ble/BLE.h"
#include "mbedtls/aes.h"


//comment out when done with debug uart, else eats batteries
#define MyDebugEnb 0



//DigitalOut led1(LED1, 1);
Ticker     ticker;

const uint16_t Periodicity = 180;   //clock periodicity used for spoof checking, should be 1800 seconds for 30minutes

//aes stuff
uint8_t src_buf[16] = {0x1,0x2,0x3,0x4,0x1,0x2,0x3,0x4,0x1,0x2,0x3,0x4,0x1,0x2,0x3,0x4};
uint8_t des_buf[16] = {0x1,0x2,0x3,0x4,0x1,0x2,0x3,0x4,0x1,0x2,0x3,0x4,0x1,0x2,0x3,0x4};
mbedtls_aes_context aes;
unsigned char iv[16] = {0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x1, 0x2};       //16-byte key
size_t input_len = 16;
size_t output_len = 0;

Serial device(p9, p11);  //nRF51822 uart :  TX=p9.  RX=p11

//data structures for tracking wake time of BLE end nodes, for to prevent spoofing
struct MAC_Birth_Record
{
    uint8_t MAC_Addr[6]; //mac[0] is low order byte in mac address
    uint16_t MAC_Time;  //The time of birth for this end node
    uint16_t Attempt_MAC_Time;
    uint8_t Attempt_Cnt;
    uint8_t Xmit_Cnt;
};
const uint8_t sizeOfSpoof = 50;
MAC_Birth_Record Spoof_Check[sizeOfSpoof];   //array tracking end node birth times
uint8_t Spoof_Ray_Tail = 0;         //array index for next record
uint8_t Received_MAC_Addr[6]; //mac[0] is low order byte in mac address
uint16_t Received_MAC_Time;
static Timer Gateway_Time;
static uint16_t Expected_MAC_Time;
uint8_t Received_Xmit_Cnt;


const uint8_t ADVisUnknown = 0;
const uint8_t ADVisNew = 1;
const uint8_t ADVisDuplicate = 2;
const uint8_t ADVisSpoof = 3;

//********************
// Takes into account 30-minute clock, wrap arounds and edge cases
// returns:
//      TRUE:  if sensor's reported time lines up to gateway's recorded time
//  Expected_MAC_Time
//*******************
bool Is_Birth_Time_Correct (uint16_t gateway_mac_time, uint16_t sensor_mac_time, uint8_t margin)
{
    bool return_val = 0;
    uint16_t current_time = (uint16_t)(Gateway_Time.read_ms()/1000);
    int16_t gateway_time_zone = current_time - sensor_mac_time;
    
    if (current_time >= sensor_mac_time)
    {
        Expected_MAC_Time = current_time - sensor_mac_time;
    }
    else
    {
        Expected_MAC_Time = (Periodicity - sensor_mac_time + current_time);
    }
    
    if (1)  //check for count==0 situations
    {
        if ( (gateway_mac_time < (Expected_MAC_Time + margin)) && (gateway_mac_time > (Expected_MAC_Time - margin)) )
        {
            return_val = 1;
        }
        else
        {
            return_val = 0;
            
        }  
    }
    
    /*
    //if positive, not a wrap around edge case
    //todo:  take into account low count, indicates reset?
    if ((gateway_time_zone >= 0) && (1))
    {
        Expected_MAC_Time = gateway_time_zone;
        if ( (gateway_mac_time < (Expected_MAC_Time + margin)) && (gateway_mac_time > (Expected_MAC_Time - margin)) )
        {
            return_val = 1;  //valid birth time
        }
        
    }
    else    //wrap around edge case, gateway_time_zone is negative
    {
        gateway_time_zone = (Periodicity - sensor_mac_time + current_time);
        Expected_MAC_Time = gateway_time_zone;                  //todo:  2 declarations??
        //if ( (Periodicity-gateway_mac_time)+current_time == sensor_mac_time) //wrong, need bands
        if ( (gateway_mac_time < (Expected_MAC_Time + margin)) && (gateway_mac_time > (Expected_MAC_Time - margin)) )
        {
            return_val = 1;
        }
        else
        {
            return_val = 0;
            
        }
        
    }
    */
    return return_val;
    
}//end Is_Birth_Time_Correct



/* **************************************
check adv against birth records to detect spoofing
add device to birth records if new
returns
    TRUE:   if device reported birth time matches real birth time
            if device is first encountered
            if X consecutive birth time matches
    FALSE:  if device reported birth time does not match
    
ToDo:  add attempts
ToDo:  wrap around of gateway clock not accounted for.  Like if Gateway_Time.read_ms() - Received_MAC_Time < 0
*/
uint8_t Is_Not_Spoofed ()
{
    uint8_t return_val = ADVisUnknown;
    //iterate through all of birth records looking for one that matches MAC address
    for (int i=0; i<sizeOfSpoof; i++)
    {
        //Search for matching MAC address
        if (Spoof_Check[i].MAC_Addr[0] == Received_MAC_Addr[0] &&
            Spoof_Check[i].MAC_Addr[1] == Received_MAC_Addr[1] &&
            Spoof_Check[i].MAC_Addr[2] == Received_MAC_Addr[2] &&
            Spoof_Check[i].MAC_Addr[3] == Received_MAC_Addr[3] &&
            Spoof_Check[i].MAC_Addr[4] == Received_MAC_Addr[4] &&
            Spoof_Check[i].MAC_Addr[5] == Received_MAC_Addr[5])
        {
            
            
            
            #if MyDebugEnb
            device.printf("found MAC address in array\r\n");
            device.printf("   Index = %d \r\n", i);
            device.printf("   MAC = %02x:%02x:%02x:%02x:%02x:%02x \r\n", Received_MAC_Addr[5],Received_MAC_Addr[4],Received_MAC_Addr[3],Received_MAC_Addr[2],Received_MAC_Addr[1],Received_MAC_Addr[0]);
            device.printf("   Received MAC Time =  %d \r\n", Received_MAC_Time);
            device.printf("   Gateway time = %d \r\n", (uint16_t)(Gateway_Time.read_ms()/1000));
            device.printf("   Array.MAC_Time =  %d \r\n", Spoof_Check[i].MAC_Time);
            device.printf("   Array.Attempt_MAC_Time =  %d \r\n", Spoof_Check[i].Attempt_MAC_Time);
            device.printf("   Array.Attempt_Cnt =  %d \r\n", Spoof_Check[i].Attempt_Cnt);
            device.printf("   Array.Xmit_Cnt =  %d \r\n", Spoof_Check[i].Xmit_Cnt);
            device.printf("   expected time =  %d \r\n", ((uint16_t)(Gateway_Time.read_ms()/1000)) - Received_MAC_Time);
            #endif
            
            //calcuate the time of birth of this bluetooth module @ gateway time zone relative to received MAC_Time.
            //int16_t expected_mac_time = ((uint16_t)(Gateway_Time.read_ms()/1000)) - Received_MAC_Time;
            //uint16_t expected_mac_time = 0;
            
            
            //todo:  this needs to be a POSITIVE VALUE, calculated by Is_MAC_Time_Correct();
            
            
            // ToDo:  there's a bug here.  Need to use count to figure out when it's a first-powered on device.  Can't use time?
            
            //*****
            // check primary Mac_Time
            //****
            if 
            (
                Is_Birth_Time_Correct(Spoof_Check[i].MAC_Time, Received_MAC_Time, 3)
            )
            {
                if (Spoof_Check[i].Xmit_Cnt != Received_Xmit_Cnt)
                {
                    return_val = ADVisNew; //MAC Time checks out and not duplicate
                    Spoof_Check[i].MAC_Time = Expected_MAC_Time;    //update birth time for this device.
                    Spoof_Check[i].Xmit_Cnt = Received_Xmit_Cnt;
                    //i = sizeOfSpoof;    //exit for loop, we've found the MAC address.  But can't do it this way.
                    //ToDo:  hey, what happens if the array holds entries w/ same MAC?  Is that possible?
                    #if MyDebugEnb
                    device.printf("Pirmary MAC Time as expected...expected=%d...Spoof_mac_tmr=%d \r\n", Expected_MAC_Time, Spoof_Check[i].MAC_Time);
                    #endif
                }
                else
                {
                    return_val = ADVisDuplicate; //MAC Time checks out and is duplicate
                }
            }//if primary MAC_time match
            else
            {
                //*****
                // check secondary Mac_Time
                //****
                #if MyDebugEnb
                device.printf("    MAC Time No Match!!...expected=%d...Spoof_mac_tmr=%d \r\n", Expected_MAC_Time, Spoof_Check[i].MAC_Time);
                
                #endif 
                
                //increment count if matches secondary
                //if ( (Spoof_Check[i].Attempt_MAC_Time < (expected_mac_time + 5)) && (Spoof_Check[i].Attempt_MAC_Time > (expected_mac_time - 5)) )
                if (Is_Birth_Time_Correct(Spoof_Check[i].Attempt_MAC_Time, Received_MAC_Time, 3))
                {
                    if (Spoof_Check[i].Xmit_Cnt != Received_Xmit_Cnt)
                    {

                        //Spoof_Check[i].Attempt_MAC_Time = expected_mac_time;
                        Spoof_Check[i].Attempt_Cnt++;
                        Spoof_Check[i].Xmit_Cnt = Received_Xmit_Cnt;
                        #if MyDebugEnb
                        device.printf("    Match on secondary, Attempt_Cnt= %d, Attempt_MAC_Time = %d, Expected_MAC_Time=%d \r\n", Spoof_Check[i].Attempt_Cnt, Spoof_Check[i].Attempt_MAC_Time, Expected_MAC_Time);
                        #endif
                        //this takes care of usecase where a module is reset after it's already been seen by gateway
                        //after 3 consecutive correlated MAC_Time attempts we assume is our long lost friend and not a spoof.
                        if (Spoof_Check[i].Attempt_Cnt >= 3)
                        {
                            return_val = ADVisNew;
                            
                            //promote this MAC_Time to primary and start accepting data @ this MAC_Time
                            Spoof_Check[i].MAC_Time = Expected_MAC_Time;
                            Spoof_Check[i].Xmit_Cnt = Received_Xmit_Cnt;
                            Spoof_Check[i].Attempt_Cnt = 0;
                        }
                        else    //Received_MAC_Time matches Attempt, but not enough times to be considered as valid.
                        {
                            return_val = ADVisSpoof;
                            
                        }
                        
                        
                    }//is not duplicate, and transmit count matches
                    else
                    {
                        #if MyDebugEnb
                        device.printf("    is Duplicate , but matches Attempt_MAC_Time matches\r\n");
                        #endif 
                        return_val = ADVisDuplicate;
                    }
                } //Recevied MAC_Time matches secondary MAC_Time
                else
                {
                    #if MyDebugEnb
                    device.printf("    No Match on secondary either, ===setting return_val = Spoof ===\r\n");
                    #endif 
                    return_val = ADVisSpoof;  //it should still be zero, so this is just for clarification
                    //at this point:  MAC matches, MAC_Time doesn't match primary nor secondary.
                    Spoof_Check[i].Attempt_Cnt = 0;  //reset secondary count

                }
                //update second backup regardless whether it matches exptected_mac_time matches secondary or not.
                Spoof_Check[i].Attempt_MAC_Time = Expected_MAC_Time;
                Spoof_Check[i].Xmit_Cnt = Received_Xmit_Cnt;
            }//if Primary MAC_Time doesn't match
            
            break;  //return_val tells us if this is a valid or not.  Don't need to search through remainder of array
        }//if matching MAC address
        else //MAC doesn't match
        {
            //MAC doesn't match and we've searched through entire record
            //let's add this MAC as new, an initialze records to match this sensor
            if (i >= (sizeOfSpoof - 1))   //we've searched through entire array and didn't find this MAC
            {
                //we've searched through the entire array and didn't find this MAC address
                //add this MAC to array, and report this as valid MAC_Time
                #if MyDebugEnb
                device.printf("MAC not found, creating new at %d \r\n", Spoof_Ray_Tail);
                device.printf("  sizeOfSpoof=%d ... and i=%d \r\n", sizeOfSpoof, i);  
                #endif
                
                return_val = ADVisNew;

                //create new entry for newly seen MAC @ tail
                // why refigure??  Spoof_Check[Spoof_Ray_Tail].MAC_Time = ((uint16_t)(Gateway_Time.read_ms()/1000)) - Received_MAC_Time;
                Spoof_Check[Spoof_Ray_Tail].MAC_Addr[0] = Received_MAC_Addr[0];
                Spoof_Check[Spoof_Ray_Tail].MAC_Addr[1] = Received_MAC_Addr[1];
                Spoof_Check[Spoof_Ray_Tail].MAC_Addr[2] = Received_MAC_Addr[2];
                Spoof_Check[Spoof_Ray_Tail].MAC_Addr[3] = Received_MAC_Addr[3];
                Spoof_Check[Spoof_Ray_Tail].MAC_Addr[4] = Received_MAC_Addr[4];
                Spoof_Check[Spoof_Ray_Tail].MAC_Addr[5] = Received_MAC_Addr[5];
                Spoof_Check[Spoof_Ray_Tail].Xmit_Cnt = Received_Xmit_Cnt;

                //calculate Expected_MAC_Time
                Is_Birth_Time_Correct (0, Received_MAC_Time, 3);
                #if MyDebugEnb
                device.printf("Expected_MAC_Time should be %d \r\n", Expected_MAC_Time);

                #endif
                Spoof_Check[Spoof_Ray_Tail].MAC_Time = Expected_MAC_Time;    //wrong!!!
                Spoof_Check[Spoof_Ray_Tail].Xmit_Cnt = Received_Xmit_Cnt;
                Spoof_Check[Spoof_Ray_Tail].Attempt_Cnt = 0;
                
                //increment tail pointer to next position or wrap around
                //todo:  might not be using the last index position of this array, not sure.  Doesn't matter IDK.
                if (Spoof_Ray_Tail >= (sizeOfSpoof-1) ) //FIFO at end of array, return to beginning
                {
                    Spoof_Ray_Tail = 0;
                }
                else
                {
                    Spoof_Ray_Tail++;
                }
                
            }//end if loop at end of array
        }//end else not maching MAC
        
    }//loop through Spoof_Check array

    
    return return_val;
}//end Is_Not_Spoofed()




void periodicCallback(void) //every 1800 seconds = 30 minutes, max for ticker.
{
#if MyDebugEnb
    device.printf("===== reset timer ===== \r\n");
#endif
    //led1 = !led1; /* Do blinky on LED1 while we're waiting for BLE events */
    //device.printf("periodic 5...");
    //device.printf("\r\n");
    Gateway_Time.reset();
}


//with the current beacon, I see 3 valid advertisements per wake period with ambient other beacons.  sometimes 4.
//even with 2 phones broadcasting at 10Hz, still see 3 adv's.
bool is_ours(const uint8_t * adv_data, const uint8_t * adv_address)
{
    //if ((adv_data[5] == adv_address[3]) && (adv_data[6] == adv_address[2]) && (adv_data[7] == adv_address[1]) && (adv_data[8] == adv_address[0]))
    if ((adv_data[5] == adv_address[3]) && (adv_data[6] == adv_address[2]))
        return 1;
    else
        return 0;
}

void advertisementCallback(const Gap::AdvertisementCallbackParams_t *params) {


    //BLE MAC Address (6 bytes):
    //      params->peerAddr[5]
    //      params->peerAddr[4]
    //      ...
    //      params->peerAddr[0]
    //RSSI = params->rssi
    //Payload
    //  params->advertisingData[#<advertisingDataLen]
    

    
    //              [<---MY DATA-->]
    //0x02      0x01    0x06    0x06    0xff    D   E   C   ?  ?
    //0         1       2       3       4       5   6   7   8   9

    if ( (params->advertisingDataLen) >= 8)
    {
        //if one of "ours", these UUID fields will be this value.  Arbitrary " 
        
        //if ( (params->advertisingData[5] == 0x44) && (params->advertisingData[6] == 0x45) && (params->advertisingData[7]==0x43) )
        //if (is_ours(params->advertisingData[5],params->advertisingData[6],params->advertisingData[7], params->advertisingData[8], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0] ))
        if (is_ours(params->advertisingData, params->peerAddr))
        {
            #if MyDebugEnb
            device.printf("----------- ADV CAll Back -----------------\r\n  ");
            #endif 
            //**************
            // Decrypt data
            //**************
            //prep array for deciphering the encrypted portion of advertisement
            //device.printf("\r\n copy adv length:  %d \r\n", params->advertisingDataLen);  //always 31
            for (int i = 0; i<16; i++)
            {
                src_buf[i]=params->advertisingData[i+15];
                #if MyDebugEnb
                //device.printf("%x ", src_buf[i]);
                #endif
            }
            #if MyDebugEnb
            //device.printf("...\r\n  ");
            #endif 

            
            mbedtls_aes_crypt_ecb( &aes, MBEDTLS_AES_DECRYPT, src_buf, des_buf );   //execution time not an issue
            
            
            #if MyDebugEnb
            
            //device.printf("decoded first 16 bytes \r\n");
            for (int i = 0; i<16; i++)
            {
                //device.printf("%02x", params->advertisingData[index]);
                if (i < 2)
                {
                   //device.printf("%x ", des_buf[i]); 
                }
                else
                {
                    //device.printf("%c ", des_buf[i]);
                }
            }
            //device.printf("done----- \r\n");
            #endif
            //save MAC address
            Received_MAC_Addr[0] = params->peerAddr[0];
            Received_MAC_Addr[1] = params->peerAddr[1];
            Received_MAC_Addr[2] = params->peerAddr[2];
            Received_MAC_Addr[3] = params->peerAddr[3];
            Received_MAC_Addr[4] = params->peerAddr[4];
            Received_MAC_Addr[5] = params->peerAddr[5];
            
            uint16_t beacon_timer = des_buf[0] | (des_buf[1] << 8);  //it's a 2 byte uint little endian
            Received_MAC_Time = beacon_timer;
            Received_Xmit_Cnt = des_buf[2];
            
            
            
            /*
            BLE Received from MAC C2F154BB0AF9
            volt:3.11,mag:1
            
            uart-transmit:
            mac:C2F154BB0AF9,rssi:##,volt:3.11,mag:1
            
            /ble/C2F154BB0AF9/rssi
            /ble/C2F154BB0AF9/1st_token
            /ble/c2F153
            
            */
            /*
            device.printf("Adv peerAddr: [%02x%02x%02x%02x%02x%02x] rssi %d, ScanResp: %u, AdvType: %u\r\n",
              params->peerAddr[5], params->peerAddr[4], params->peerAddr[3], params->peerAddr[2], params->peerAddr[1], params->peerAddr[0],
              params->rssi, params->isScanResponse, params->type);
            */
            
            // punch out the data as json
            //---------------------------------------
            // 1.  MAC and RSSI
            // "mac":xxxxxxxx,rssi:xx,
            //---------------------------------------
            uint8_t ADV_Result = Is_Not_Spoofed();
            #if MyDebugEnb
            device.printf("--------ADV_Result = %d", ADV_Result);
            #endif


            if (ADV_Result == ADVisNew)
            {
                device.printf("{\"mac\":\"%02x%02x%02x%02x%02x%02x\",\"rssi\":%d,",
                    params->peerAddr[5], 
                    params->peerAddr[4], 
                    params->peerAddr[3], 
                    params->peerAddr[2], 
                    params->peerAddr[1], 
                    params->peerAddr[0],
                    params->rssi
                    );
                //mac:"c2f154bb0af9"
    
                //---------------------------------------
                // 2.  Volt
                // "volt":3.03,
                //---------------------------------------
                device.printf("\"volt\":%c%c%c%c,",
                    params->advertisingData[9],
                    params->advertisingData[10],
                    params->advertisingData[11],
                    params->advertisingData[12]);
                
    
    
    
                //---------------------------------------
                // 3.  clock
                // "tmr":xxx,
                //---------------------------------------
                //print clock
                //     "tmr":3232,
                //---------------------------------------
                
                device.printf("\"tmr\":%d,", beacon_timer);
                //device.printf("\"tmr\":%d,", des_buf[0]);
                device.printf("\"xmit_cnt\":%d,", Received_Xmit_Cnt);
                
                //---------------------------------------
                // 4.  rest of payload as json
                // "tmr":xxx,
                //---------------------------------------
                for (int i = 3; i< 16; i++)
                {
                    //device.printf("%02x", params->advertisingData[index]);
                    device.printf("%c", des_buf[i]);    //print as character
                }
    
    
    
                /*
                for (unsigned index = 8; index < params->advertisingDataLen; index++)
                {
                    //device.printf("%02x", params->advertisingData[index]);
                    device.printf("%c", params->advertisingData[index]);
                }
                */
                
                device.printf("}");
                device.printf("\r\n");
                
                /*
                mac:c2f154bb0af9,rssi:-55,volt:3.05,mag:1
                mac:c2f154bb0af9,rssi:-67,volt:3.05,mag:1
                mac:c2f154bb0af9,rssi:-60,volt:3.07,mag:0
                */
            }
            if (ADV_Result == ADVisDuplicate)
            {
                device.printf("{\"mac\":\"%02x%02x%02x%02x%02x%02x\",\"duplicate\": %d}\r\n",
                    params->peerAddr[5], 
                    params->peerAddr[4], 
                    params->peerAddr[3], 
                    params->peerAddr[2], 
                    params->peerAddr[1], 
                    params->peerAddr[0],
                    ADV_Result);
            }
            if (ADV_Result == ADVisSpoof)
            {
                device.printf("{\"mac\":\"%02x%02x%02x%02x%02x%02x\",\"spoof\": %d}\r\n",
                    params->peerAddr[5], 
                    params->peerAddr[4], 
                    params->peerAddr[3], 
                    params->peerAddr[2], 
                    params->peerAddr[1], 
                    params->peerAddr[0],
                    ADV_Result);
            }
            if (ADV_Result == ADVisUnknown)
            {
                device.printf("{\"mac\":\"%02x%02x%02x%02x%02x%02x\",\"unknown\": %d}\r\n",
                    params->peerAddr[5], 
                    params->peerAddr[4], 
                    params->peerAddr[3], 
                    params->peerAddr[2], 
                    params->peerAddr[1], 
                    params->peerAddr[0],
                    ADV_Result);            
            }
        }//end if it's our adv
    }//end if advertisingDataLen

}//end advertisementCallback



/**
 * This function is called when the ble initialization process has failed
 */
void onBleInitError(BLE &ble, ble_error_t error)
{
    /* Initialization error handling should go here */
    device.printf("periodic callback ");
    device.printf("\r\n");
}

/**
 * Callback triggered when the ble initialization process has finished
 */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;

    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }

    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }
 
    // in ms.  Duty cycle = (interval / window);  200ms/500ms = 40%
    ble.gap().setScanParams(500 /* scan interval */, 200 /* scan window */);
    ble.gap().startScan(advertisementCallback);
}

int main(void)
{
    for (int i=0; i<sizeOfSpoof; i++)
    {
        Spoof_Check[i].MAC_Addr[0]= 0;
        Spoof_Check[i].MAC_Addr[1]= 0;
        Spoof_Check[i].MAC_Addr[2]= 0;
        Spoof_Check[i].MAC_Time = 0;
        Spoof_Check[i].Attempt_MAC_Time = 0;
        Spoof_Check[i].Attempt_Cnt = 0;
        Spoof_Check[i].Attempt_Cnt = 0;
        Spoof_Check[i].Xmit_Cnt = 0;
    }
    
    Gateway_Time.start();
    device.baud(9600);
    device.printf("started main 06 hashed observer... ");
    device.printf("\r\n");
    ticker.attach(periodicCallback, Periodicity);
    
    //mbedtls_aes_init(&aes);
    mbedtls_aes_setkey_dec( &aes, iv, 128 );
    //mbedtls_aes_crypt_ecb( &aes, MBEDTLS_AES_DECRYPT, input, output );

    BLE &ble = BLE::Instance();
    ble.init(bleInitComplete);

    while (true) {
        ble.waitForEvent();
    }
}