TLMFrame.cpp

00001 /* mbed Microcontroller Library
00002  * Copyright (c) 2006-2015 ARM Limited
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #include "TLMFrame.h"
00018 #include "EddystoneService.h"
00019 
00020 TLMFrame::TLMFrame(uint8_t  tlmVersionIn,
00021                    uint16_t tlmBatteryVoltageIn,
00022                    uint16_t tlmBeaconTemperatureIn,
00023                    uint32_t tlmPduCountIn,
00024                    uint32_t tlmTimeSinceBootIn) :
00025     tlmVersion(tlmVersionIn),
00026     lastTimeSinceBootRead(0),
00027     tlmBatteryVoltage(tlmBatteryVoltageIn),
00028     tlmBeaconTemperature(tlmBeaconTemperatureIn),
00029     tlmPduCount(tlmPduCountIn),
00030     tlmTimeSinceBoot(tlmTimeSinceBootIn)
00031 {
00032 }
00033 
00034 void TLMFrame::setTLMData(uint8_t tlmVersionIn)
00035 {
00036     /* According to the Eddystone spec BatteryVoltage is 0 and
00037      * BeaconTemperature is 0x8000 if not supported
00038      */
00039     tlmVersion           = tlmVersionIn;
00040     tlmBatteryVoltage    = 0;
00041     tlmBeaconTemperature = 0x8000;
00042     tlmPduCount          = 0;
00043     tlmTimeSinceBoot     = 0;
00044 }
00045 
00046 void TLMFrame::setData(uint8_t *rawFrame)  // add eidTime - a 4 byte quantity
00047 {
00048     size_t index = 0;
00049     rawFrame[index++] = EDDYSTONE_UUID_SIZE + FRAME_SIZE_TLM; // Length of Frame
00050     rawFrame[index++] = EDDYSTONE_UUID[0];                    // 16-bit Eddystone UUID
00051     rawFrame[index++] = EDDYSTONE_UUID[1];
00052     rawFrame[index++] = FRAME_TYPE_TLM;                       // Eddystone frame type = Telemetry
00053     rawFrame[index++] = tlmVersion;                           // TLM Version Number
00054     rawFrame[index++] = (uint8_t)(tlmBatteryVoltage >> 8);    // Battery Voltage[0]
00055     rawFrame[index++] = (uint8_t)(tlmBatteryVoltage >> 0);    // Battery Voltage[1]
00056     rawFrame[index++] = (uint8_t)(tlmBeaconTemperature >> 8); // Beacon Temp[0]
00057     rawFrame[index++] = (uint8_t)(tlmBeaconTemperature >> 0); // Beacon Temp[1]
00058     rawFrame[index++] = (uint8_t)(tlmPduCount >> 24);         // PDU Count [0]
00059     rawFrame[index++] = (uint8_t)(tlmPduCount >> 16);         // PDU Count [1]
00060     rawFrame[index++] = (uint8_t)(tlmPduCount >> 8);          // PDU Count [2]
00061     rawFrame[index++] = (uint8_t)(tlmPduCount >> 0);          // PDU Count [3]
00062     rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 24);    // Time Since Boot [0]
00063     rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 16);    // Time Since Boot [1]
00064     rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 8);     // Time Since Boot [2]
00065     rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 0);     // Time Since Boot [3]
00066 }
00067 
00068 void TLMFrame::encryptData(uint8_t* rawFrame, uint8_t* eidIdentityKey, uint8_t rotationPeriodExp, uint32_t beaconTimeSecs) {
00069     // uint8_t newinput[ETLM_DATA_LEN];        // DEBUG ONLY
00070         
00071     // Initialize AES data
00072     mbedtls_aes_context ctx;
00073     mbedtls_aes_init(&ctx); 
00074     mbedtls_aes_setkey_enc(&ctx, eidIdentityKey, sizeof(EidIdentityKey_t) *8 );
00075     // Change the TLM version number to the encrypted version
00076     rawFrame[VERSION_OFFSET] = ETLM_VERSION; // Encrypted TLM Version number
00077     // Create EAX Params
00078     uint8_t nonce[ETLM_NONCE_LEN];
00079     // Calculate the 48-bit nonce
00080     generateEtlmNonce(nonce, rotationPeriodExp, beaconTimeSecs);
00081  
00082     uint8_t* input = rawFrame + DATA_OFFSET;  // array size 12
00083     uint8_t output[ETLM_DATA_LEN]; // array size 16 (4 bytes are added: SALT[2], MIC[2])
00084     memset(output, 0, ETLM_DATA_LEN);
00085     uint8_t emptyHeader[1]; // Empty header
00086     LOG(("EIDIdentityKey=\r\n")); EddystoneService::logPrintHex(eidIdentityKey, 16);
00087     LOG(("TLM input=\r\n")); EddystoneService::logPrintHex(input, 12);
00088     LOG(("ETLM SALT=\r\n")); EddystoneService::logPrintHex(nonce+4, 2);
00089     LOG(("ETLM Nonce=\r\n")); EddystoneService::logPrintHex(nonce, 6);
00090     // Encrypt the TLM to ETLM
00091     eddy_aes_authcrypt_eax(&ctx, MBEDTLS_AES_ENCRYPT, nonce, sizeof(nonce), emptyHeader, 0, TLM_DATA_LEN, input, output, output + MIC_OFFSET, MIC_LEN);
00092     // memcpy(newinput, output, ETLM_DATA_LEN); // DEBUG ONLY
00093     // Only use first 2 bytes of Nonce
00094     output[SALT_OFFSET] = nonce[4]; // Nonce MSB
00095     output[SALT_OFFSET+1] = nonce[5]; // Nonce LSB
00096     LOG(("ETLM output+SALT=\r\n")); EddystoneService::logPrintHex(output, 16);
00097     // copy the encrypted payload to the output
00098     memcpy((rawFrame + DATA_OFFSET), output, ETLM_DATA_LEN);
00099         
00100     /* 
00101     // DEBUG ONLY TO CHECK DECRYPT==ENCRYPT
00102     uint8_t buf[ETLM_DATA_LEN];
00103     memset(buf, 0, ETLM_DATA_LEN);
00104     int ret = eddy_aes_authcrypt_eax(&ctx, MBEDTLS_AES_DECRYPT, nonce, sizeof(nonce), emptyHeader, 0, TLM_DATA_LEN, newinput, buf, buf + MIC_OFFSET, MIC_LEN);
00105     LOG(("ETLM Decoder ret=%d buf=\r\n", ret)); EddystoneService::logPrintHex(buf, 16);
00106     */
00107         
00108     // fix the frame length to the encrypted length
00109     rawFrame[FRAME_LEN_OFFSET] = FRAME_SIZE_ETLM + EDDYSTONE_UUID_SIZE; 
00110     // Free the AES data struture
00111     mbedtls_aes_free(&ctx);
00112 }
00113     
00114 
00115 size_t TLMFrame::getRawFrameSize(uint8_t* rawFrame)
00116 {
00117     return rawFrame[FRAME_LEN_OFFSET];
00118 }
00119 
00120 uint8_t* TLMFrame::getData(uint8_t* rawFrame) 
00121 {
00122     if (rawFrame[VERSION_OFFSET] == TLM_VERSION) {
00123         setData(rawFrame);
00124     }
00125     return &(rawFrame[TLM_DATA_OFFSET]);
00126 }
00127 
00128 uint8_t TLMFrame::getDataLength(uint8_t* rawFrame)
00129 {
00130     return rawFrame[FRAME_LEN_OFFSET] - EDDYSTONE_UUID_LEN;
00131 }
00132 
00133 uint8_t* TLMFrame::getAdvFrame(uint8_t* rawFrame){
00134     return &(rawFrame[ADV_FRAME_OFFSET]);
00135 }
00136 
00137 uint8_t TLMFrame::getAdvFrameLength(uint8_t* rawFrame){
00138     return rawFrame[FRAME_LEN_OFFSET];
00139 }
00140 
00141 void TLMFrame::updateTimeSinceBoot(uint32_t nowInMillis)
00142 {
00143     // Measured in tenths of a second
00144     tlmTimeSinceBoot      += (nowInMillis - lastTimeSinceBootRead) / 100;
00145     lastTimeSinceBootRead  = nowInMillis;
00146 }
00147 
00148 void TLMFrame::updateBatteryVoltage(uint16_t tlmBatteryVoltageIn)
00149 {
00150     tlmBatteryVoltage = tlmBatteryVoltageIn;
00151 }
00152 
00153 void TLMFrame::updateBeaconTemperature(uint16_t tlmBeaconTemperatureIn)
00154 {
00155     tlmBeaconTemperature = tlmBeaconTemperatureIn;
00156 }
00157 
00158 void TLMFrame::updatePduCount(void)
00159 {
00160     tlmPduCount++;
00161 }
00162 
00163 uint16_t TLMFrame::getBatteryVoltage(void) const
00164 {
00165     return tlmBatteryVoltage;
00166 }
00167 
00168 uint16_t TLMFrame::getBeaconTemperature(void) const
00169 {
00170     return tlmBeaconTemperature;
00171 }
00172 
00173 uint8_t TLMFrame::getTLMVersion(void) const
00174 {
00175     return tlmVersion;
00176 }
00177 
00178 int TLMFrame::generateEtlmNonce(uint8_t* nonce, uint8_t rotationPeriodExp, uint32_t beaconTimeSecs) {
00179     int rc = 0;
00180     if (sizeof(nonce) != ETLM_NONCE_LEN) {
00181       rc = ETLM_NONCE_INVALID_LEN; 
00182     }
00183     uint32_t scaledTime = (beaconTimeSecs >> rotationPeriodExp) << rotationPeriodExp;
00184     int index = 0;
00185     nonce[index++] = (scaledTime  >> 24) & 0xff;
00186     nonce[index++] = (scaledTime >> 16) & 0xff;
00187     nonce[index++] = (scaledTime >> 8) & 0xff;
00188     nonce[index++] = scaledTime & 0xff;
00189     EddystoneService::generateRandom(nonce + index, SALT_LEN);
00190     return rc;
00191 }
00192