Roy Want / Mbed OS beaconCompileReadyFork

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

source/TLMFrame.cpp@0:ed0152b5c495, 2016-09-19 (annotated)

Committer:
roywant
Date:
Mon Sep 19 00:59:11 2016 +0000
Revision:
0:ed0152b5c495
Initial commit

Who changed what in which revision?

UserRevisionLine numberNew contents of line
roywant 0:ed0152b5c495 1 /* mbed Microcontroller Library
roywant 0:ed0152b5c495 2 * Copyright (c) 2006-2015 ARM Limited
roywant 0:ed0152b5c495 3 *
roywant 0:ed0152b5c495 4 * Licensed under the Apache License, Version 2.0 (the "License");
roywant 0:ed0152b5c495 5 * you may not use this file except in compliance with the License.
roywant 0:ed0152b5c495 6 * You may obtain a copy of the License at
roywant 0:ed0152b5c495 7 *
roywant 0:ed0152b5c495 8 * http://www.apache.org/licenses/LICENSE-2.0
roywant 0:ed0152b5c495 9 *
roywant 0:ed0152b5c495 10 * Unless required by applicable law or agreed to in writing, software
roywant 0:ed0152b5c495 11 * distributed under the License is distributed on an "AS IS" BASIS,
roywant 0:ed0152b5c495 12 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
roywant 0:ed0152b5c495 13 * See the License for the specific language governing permissions and
roywant 0:ed0152b5c495 14 * limitations under the License.
roywant 0:ed0152b5c495 15 */
roywant 0:ed0152b5c495 16
roywant 0:ed0152b5c495 17 #include "TLMFrame.h"
roywant 0:ed0152b5c495 18 #include "EddystoneService.h"
roywant 0:ed0152b5c495 19
roywant 0:ed0152b5c495 20 TLMFrame::TLMFrame(uint8_t tlmVersionIn,
roywant 0:ed0152b5c495 21 uint16_t tlmBatteryVoltageIn,
roywant 0:ed0152b5c495 22 uint16_t tlmBeaconTemperatureIn,
roywant 0:ed0152b5c495 23 uint32_t tlmPduCountIn,
roywant 0:ed0152b5c495 24 uint32_t tlmTimeSinceBootIn) :
roywant 0:ed0152b5c495 25 tlmVersion(tlmVersionIn),
roywant 0:ed0152b5c495 26 lastTimeSinceBootRead(0),
roywant 0:ed0152b5c495 27 tlmBatteryVoltage(tlmBatteryVoltageIn),
roywant 0:ed0152b5c495 28 tlmBeaconTemperature(tlmBeaconTemperatureIn),
roywant 0:ed0152b5c495 29 tlmPduCount(tlmPduCountIn),
roywant 0:ed0152b5c495 30 tlmTimeSinceBoot(tlmTimeSinceBootIn)
roywant 0:ed0152b5c495 31 {
roywant 0:ed0152b5c495 32 }
roywant 0:ed0152b5c495 33
roywant 0:ed0152b5c495 34 void TLMFrame::setTLMData(uint8_t tlmVersionIn)
roywant 0:ed0152b5c495 35 {
roywant 0:ed0152b5c495 36 /* According to the Eddystone spec BatteryVoltage is 0 and
roywant 0:ed0152b5c495 37 * BeaconTemperature is 0x8000 if not supported
roywant 0:ed0152b5c495 38 */
roywant 0:ed0152b5c495 39 tlmVersion = tlmVersionIn;
roywant 0:ed0152b5c495 40 tlmBatteryVoltage = 0;
roywant 0:ed0152b5c495 41 tlmBeaconTemperature = 0x8000;
roywant 0:ed0152b5c495 42 tlmPduCount = 0;
roywant 0:ed0152b5c495 43 tlmTimeSinceBoot = 0;
roywant 0:ed0152b5c495 44 }
roywant 0:ed0152b5c495 45
roywant 0:ed0152b5c495 46 void TLMFrame::setData(uint8_t *rawFrame) // add eidTime - a 4 byte quantity
roywant 0:ed0152b5c495 47 {
roywant 0:ed0152b5c495 48 size_t index = 0;
roywant 0:ed0152b5c495 49 rawFrame[index++] = EDDYSTONE_UUID_SIZE + FRAME_SIZE_TLM; // Length of Frame
roywant 0:ed0152b5c495 50 rawFrame[index++] = EDDYSTONE_UUID[0]; // 16-bit Eddystone UUID
roywant 0:ed0152b5c495 51 rawFrame[index++] = EDDYSTONE_UUID[1];
roywant 0:ed0152b5c495 52 rawFrame[index++] = FRAME_TYPE_TLM; // Eddystone frame type = Telemetry
roywant 0:ed0152b5c495 53 rawFrame[index++] = tlmVersion; // TLM Version Number
roywant 0:ed0152b5c495 54 rawFrame[index++] = (uint8_t)(tlmBatteryVoltage >> 8); // Battery Voltage[0]
roywant 0:ed0152b5c495 55 rawFrame[index++] = (uint8_t)(tlmBatteryVoltage >> 0); // Battery Voltage[1]
roywant 0:ed0152b5c495 56 rawFrame[index++] = (uint8_t)(tlmBeaconTemperature >> 8); // Beacon Temp[0]
roywant 0:ed0152b5c495 57 rawFrame[index++] = (uint8_t)(tlmBeaconTemperature >> 0); // Beacon Temp[1]
roywant 0:ed0152b5c495 58 rawFrame[index++] = (uint8_t)(tlmPduCount >> 24); // PDU Count [0]
roywant 0:ed0152b5c495 59 rawFrame[index++] = (uint8_t)(tlmPduCount >> 16); // PDU Count [1]
roywant 0:ed0152b5c495 60 rawFrame[index++] = (uint8_t)(tlmPduCount >> 8); // PDU Count [2]
roywant 0:ed0152b5c495 61 rawFrame[index++] = (uint8_t)(tlmPduCount >> 0); // PDU Count [3]
roywant 0:ed0152b5c495 62 rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 24); // Time Since Boot [0]
roywant 0:ed0152b5c495 63 rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 16); // Time Since Boot [1]
roywant 0:ed0152b5c495 64 rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 8); // Time Since Boot [2]
roywant 0:ed0152b5c495 65 rawFrame[index++] = (uint8_t)(tlmTimeSinceBoot >> 0); // Time Since Boot [3]
roywant 0:ed0152b5c495 66 }
roywant 0:ed0152b5c495 67
roywant 0:ed0152b5c495 68 void TLMFrame::encryptData(uint8_t* rawFrame, uint8_t* eidIdentityKey, uint8_t rotationPeriodExp, uint32_t beaconTimeSecs) {
roywant 0:ed0152b5c495 69 // uint8_t newinput[ETLM_DATA_LEN]; // DEBUG ONLY
roywant 0:ed0152b5c495 70
roywant 0:ed0152b5c495 71 // Initialize AES data
roywant 0:ed0152b5c495 72 mbedtls_aes_context ctx;
roywant 0:ed0152b5c495 73 mbedtls_aes_init(&ctx);
roywant 0:ed0152b5c495 74 mbedtls_aes_setkey_enc(&ctx, eidIdentityKey, sizeof(EidIdentityKey_t) *8 );
roywant 0:ed0152b5c495 75 // Change the TLM version number to the encrypted version
roywant 0:ed0152b5c495 76 rawFrame[VERSION_OFFSET] = ETLM_VERSION; // Encrypted TLM Version number
roywant 0:ed0152b5c495 77 // Create EAX Params
roywant 0:ed0152b5c495 78 uint8_t nonce[ETLM_NONCE_LEN];
roywant 0:ed0152b5c495 79 // Calculate the 48-bit nonce
roywant 0:ed0152b5c495 80 generateEtlmNonce(nonce, rotationPeriodExp, beaconTimeSecs);
roywant 0:ed0152b5c495 81
roywant 0:ed0152b5c495 82 uint8_t* input = rawFrame + DATA_OFFSET; // array size 12
roywant 0:ed0152b5c495 83 uint8_t output[ETLM_DATA_LEN]; // array size 16 (4 bytes are added: SALT[2], MIC[2])
roywant 0:ed0152b5c495 84 memset(output, 0, ETLM_DATA_LEN);
roywant 0:ed0152b5c495 85 uint8_t emptyHeader[1]; // Empty header
roywant 0:ed0152b5c495 86 LOG(("EIDIdentityKey=\r\n")); EddystoneService::logPrintHex(eidIdentityKey, 16);
roywant 0:ed0152b5c495 87 LOG(("TLM input=\r\n")); EddystoneService::logPrintHex(input, 12);
roywant 0:ed0152b5c495 88 LOG(("ETLM SALT=\r\n")); EddystoneService::logPrintHex(nonce+4, 2);
roywant 0:ed0152b5c495 89 LOG(("ETLM Nonce=\r\n")); EddystoneService::logPrintHex(nonce, 6);
roywant 0:ed0152b5c495 90 // Encrypt the TLM to ETLM
roywant 0:ed0152b5c495 91 eddy_aes_authcrypt_eax(&ctx, MBEDTLS_AES_ENCRYPT, nonce, sizeof(nonce), emptyHeader, 0, TLM_DATA_LEN, input, output, output + MIC_OFFSET, MIC_LEN);
roywant 0:ed0152b5c495 92 // memcpy(newinput, output, ETLM_DATA_LEN); // DEBUG ONLY
roywant 0:ed0152b5c495 93 // Only use first 2 bytes of Nonce
roywant 0:ed0152b5c495 94 output[SALT_OFFSET] = nonce[4]; // Nonce MSB
roywant 0:ed0152b5c495 95 output[SALT_OFFSET+1] = nonce[5]; // Nonce LSB
roywant 0:ed0152b5c495 96 LOG(("ETLM output+SALT=\r\n")); EddystoneService::logPrintHex(output, 16);
roywant 0:ed0152b5c495 97 // copy the encrypted payload to the output
roywant 0:ed0152b5c495 98 memcpy((rawFrame + DATA_OFFSET), output, ETLM_DATA_LEN);
roywant 0:ed0152b5c495 99
roywant 0:ed0152b5c495 100 /*
roywant 0:ed0152b5c495 101 // DEBUG ONLY TO CHECK DECRYPT==ENCRYPT
roywant 0:ed0152b5c495 102 uint8_t buf[ETLM_DATA_LEN];
roywant 0:ed0152b5c495 103 memset(buf, 0, ETLM_DATA_LEN);
roywant 0:ed0152b5c495 104 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);
roywant 0:ed0152b5c495 105 LOG(("ETLM Decoder ret=%d buf=\r\n", ret)); EddystoneService::logPrintHex(buf, 16);
roywant 0:ed0152b5c495 106 */
roywant 0:ed0152b5c495 107
roywant 0:ed0152b5c495 108 // fix the frame length to the encrypted length
roywant 0:ed0152b5c495 109 rawFrame[FRAME_LEN_OFFSET] = FRAME_SIZE_ETLM + EDDYSTONE_UUID_SIZE;
roywant 0:ed0152b5c495 110 // Free the AES data struture
roywant 0:ed0152b5c495 111 mbedtls_aes_free(&ctx);
roywant 0:ed0152b5c495 112 }
roywant 0:ed0152b5c495 113
roywant 0:ed0152b5c495 114
roywant 0:ed0152b5c495 115 size_t TLMFrame::getRawFrameSize(uint8_t* rawFrame)
roywant 0:ed0152b5c495 116 {
roywant 0:ed0152b5c495 117 return rawFrame[FRAME_LEN_OFFSET];
roywant 0:ed0152b5c495 118 }
roywant 0:ed0152b5c495 119
roywant 0:ed0152b5c495 120 uint8_t* TLMFrame::getData(uint8_t* rawFrame)
roywant 0:ed0152b5c495 121 {
roywant 0:ed0152b5c495 122 if (rawFrame[VERSION_OFFSET] == TLM_VERSION) {
roywant 0:ed0152b5c495 123 setData(rawFrame);
roywant 0:ed0152b5c495 124 }
roywant 0:ed0152b5c495 125 return &(rawFrame[TLM_DATA_OFFSET]);
roywant 0:ed0152b5c495 126 }
roywant 0:ed0152b5c495 127
roywant 0:ed0152b5c495 128 uint8_t TLMFrame::getDataLength(uint8_t* rawFrame)
roywant 0:ed0152b5c495 129 {
roywant 0:ed0152b5c495 130 return rawFrame[FRAME_LEN_OFFSET] - EDDYSTONE_UUID_LEN;
roywant 0:ed0152b5c495 131 }
roywant 0:ed0152b5c495 132
roywant 0:ed0152b5c495 133 uint8_t* TLMFrame::getAdvFrame(uint8_t* rawFrame){
roywant 0:ed0152b5c495 134 return &(rawFrame[ADV_FRAME_OFFSET]);
roywant 0:ed0152b5c495 135 }
roywant 0:ed0152b5c495 136
roywant 0:ed0152b5c495 137 uint8_t TLMFrame::getAdvFrameLength(uint8_t* rawFrame){
roywant 0:ed0152b5c495 138 return rawFrame[FRAME_LEN_OFFSET];
roywant 0:ed0152b5c495 139 }
roywant 0:ed0152b5c495 140
roywant 0:ed0152b5c495 141 void TLMFrame::updateTimeSinceBoot(uint32_t nowInMillis)
roywant 0:ed0152b5c495 142 {
roywant 0:ed0152b5c495 143 // Measured in tenths of a second
roywant 0:ed0152b5c495 144 tlmTimeSinceBoot += (nowInMillis - lastTimeSinceBootRead) / 100;
roywant 0:ed0152b5c495 145 lastTimeSinceBootRead = nowInMillis;
roywant 0:ed0152b5c495 146 }
roywant 0:ed0152b5c495 147
roywant 0:ed0152b5c495 148 void TLMFrame::updateBatteryVoltage(uint16_t tlmBatteryVoltageIn)
roywant 0:ed0152b5c495 149 {
roywant 0:ed0152b5c495 150 tlmBatteryVoltage = tlmBatteryVoltageIn;
roywant 0:ed0152b5c495 151 }
roywant 0:ed0152b5c495 152
roywant 0:ed0152b5c495 153 void TLMFrame::updateBeaconTemperature(uint16_t tlmBeaconTemperatureIn)
roywant 0:ed0152b5c495 154 {
roywant 0:ed0152b5c495 155 tlmBeaconTemperature = tlmBeaconTemperatureIn;
roywant 0:ed0152b5c495 156 }
roywant 0:ed0152b5c495 157
roywant 0:ed0152b5c495 158 void TLMFrame::updatePduCount(void)
roywant 0:ed0152b5c495 159 {
roywant 0:ed0152b5c495 160 tlmPduCount++;
roywant 0:ed0152b5c495 161 }
roywant 0:ed0152b5c495 162
roywant 0:ed0152b5c495 163 uint16_t TLMFrame::getBatteryVoltage(void) const
roywant 0:ed0152b5c495 164 {
roywant 0:ed0152b5c495 165 return tlmBatteryVoltage;
roywant 0:ed0152b5c495 166 }
roywant 0:ed0152b5c495 167
roywant 0:ed0152b5c495 168 uint16_t TLMFrame::getBeaconTemperature(void) const
roywant 0:ed0152b5c495 169 {
roywant 0:ed0152b5c495 170 return tlmBeaconTemperature;
roywant 0:ed0152b5c495 171 }
roywant 0:ed0152b5c495 172
roywant 0:ed0152b5c495 173 uint8_t TLMFrame::getTLMVersion(void) const
roywant 0:ed0152b5c495 174 {
roywant 0:ed0152b5c495 175 return tlmVersion;
roywant 0:ed0152b5c495 176 }
roywant 0:ed0152b5c495 177
roywant 0:ed0152b5c495 178 int TLMFrame::generateEtlmNonce(uint8_t* nonce, uint8_t rotationPeriodExp, uint32_t beaconTimeSecs) {
roywant 0:ed0152b5c495 179 int rc = 0;
roywant 0:ed0152b5c495 180 if (sizeof(nonce) != ETLM_NONCE_LEN) {
roywant 0:ed0152b5c495 181 rc = ETLM_NONCE_INVALID_LEN;
roywant 0:ed0152b5c495 182 }
roywant 0:ed0152b5c495 183 uint32_t scaledTime = (beaconTimeSecs >> rotationPeriodExp) << rotationPeriodExp;
roywant 0:ed0152b5c495 184 int index = 0;
roywant 0:ed0152b5c495 185 nonce[index++] = (scaledTime >> 24) & 0xff;
roywant 0:ed0152b5c495 186 nonce[index++] = (scaledTime >> 16) & 0xff;
roywant 0:ed0152b5c495 187 nonce[index++] = (scaledTime >> 8) & 0xff;
roywant 0:ed0152b5c495 188 nonce[index++] = scaledTime & 0xff;
roywant 0:ed0152b5c495 189 EddystoneService::generateRandom(nonce + index, SALT_LEN);
roywant 0:ed0152b5c495 190 return rc;
roywant 0:ed0152b5c495 191 }
roywant 0:ed0152b5c495 192