Skip to content

Commit

Permalink
Hardened comms protocol - can not recover from dropped packets
Browse files Browse the repository at this point in the history 
while maintaining high throughput.
  • Loading branch information
@clydebarrow
clydebarrow committed Apr 6, 2017
1 parent fde45e5 commit cc791ab
Show file tree
Hide file tree
Showing 8 changed files with 157 additions and 49 deletions.
6 changes: 3 additions & 3 deletions bootload/CMakeLists.txt
View file
Original file line number Diff line number Diff line change
Expand Up @@ -35,8 +35,9 @@ set(BIN_FILE ${CMAKE_BINARY_DIR}/${PROJECT_NAME}.bin)

file(GLOB_RECURSE USER_SOURCES "src/*.c")
file(GLOB_RECURSE USER_HEADERS "inc/*.h")
file(STRINGS ${OTA_SERVICE_UUID_FILE} OTA_SERVICE)
file(STRINGS ${OTA_SERVICE_UUID_FILE} OTA_SERVICE_UUID)
file(STRINGS ${OTA_KEY_FILE} OTA_KEY LIMIT_INPUT 64)
configure_file(${GATT_XML} ${GATT_I})

IF (CMAKE_BUILD_TYPE MATCHES Debug)
file(GLOB_RECURSE RTT_LIBS "libs/SEGGER_RTT_V612j/RTT/*.c")
Expand All @@ -57,8 +58,7 @@ set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_EXE_LINKER_FLAGS} -Wl,-Map=${PROJECT_NAME}.m

add_custom_command(
OUTPUT ${GATT_OUTPUTS}
DEPENDS ${GATT_SRC} ${GATT_XML}
COMMAND ${CMAKE_C_COMPILER} -E -C -P -C -DOTA_SERVICE_UUID=\\\"${OTA_SERVICE}\\\" - < ${GATT_XML} > ${GATT_I}
DEPENDS ${GATT_SRC} ${GATT_XML} ${GATT_I}
COMMAND ${BGBUILD_CMD} -gn ${GATT_SRC})
set_source_files_properties(${GATT_OUTPUTS} PROPERTIES GENERATED TRUE)

Expand Down
1 change: 1 addition & 0 deletions bootload/gatt/constants
View file
Original file line number Diff line number Diff line change
Expand Up @@ -4,3 +4,4 @@ hwrev 18
fwrev 20
ota_control 23
ota_data 26
ota_progress 29
9 changes: 7 additions & 2 deletions bootload/gatt/gatt.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -51,7 +51,7 @@
</characteristic>
</service>

<service uuid=OTA_SERVICE_UUID>
<service uuid="${OTA_SERVICE_UUID}">
<description>EFR32BG OTA</description>
<characteristic uuid="95301001-963F-46B1-B801-0B23E8904835" id="ota_control">
<properties write="true"/>
Expand All @@ -60,9 +60,14 @@
</characteristic>
<characteristic uuid="95301002-963F-46B1-B801-0B23E8904835" id="ota_data">
<properties write="true" write_no_response="true"/>
<value type="user" length="32"/>
<value type="user" length="64"/>
<description>OTA DATA</description>
</characteristic>
<characteristic uuid="95301003-963F-46B1-B801-0B23E8904835" id="ota_progress">
<properties notify="true" />
<value type="user" length="16"/>
<description>OTA Progress</description>
</characteristic>
</service>


Expand Down
1 change: 1 addition & 0 deletions bootload/gatt/gatt_db.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -15,5 +15,6 @@ extern const struct bg_gattdb_def bg_gattdb_data;
#define GATTDB_fwrev 20
#define GATTDB_ota_control 23
#define GATTDB_ota_data 26
#define GATTDB_ota_progress 29

#endif
8 changes: 8 additions & 0 deletions bootload/inc/dfu.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,13 @@
#define KEY_LEN (256/8) // length of key
#define DIGEST_LEN (256/8) // length of SHA256 digest


#define MIN_CONN_INTERVAL 9 // 7.5ms
#define MAX_CONN_INTERVAL 9 // 7.5ms
#define LATENCY 40 // max number of connection attempts we can skip. This is set high
#define SUPERV_TIMEOUT 300 // 30s
#define MAX_MTU 72 // max mtu

extern blat_t __UserStart;
#define USER_BLAT (&__UserStart)
extern bool processCtrlPacket(uint8 * packet); // process a control packet. Return true if accepted
Expand All @@ -40,6 +47,7 @@ extern bool enterDfu;
extern bool doReset;
extern const unsigned char ota_key[KEY_LEN];
extern unsigned char deKey[KEY_LEN];
extern uint8 currentConnection;

#define DFU_ENTRY_VECTOR 7 // index into vector table for EnterDFU_Handler

Expand Down
128 changes: 102 additions & 26 deletions bootload/src/dfu.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,14 @@
#include <io.h>
#include <flash.h>
#include <em_crypto.h>
#include <native_gecko.h>
#include <gatt_db.h>

static uint32 dataAddress, dataCount, digestSize, digestAddress;
#define PROG_INCREMENT 25
#define DFU_RESYNC 1
#define DIGEST_FAILED 2

static uint32 dataAddress, digestSize, digestAddress, baseAddress, dataCount;
static uint32 ivLen, digestLen;
static uint8_t iv[IV_LEN];
static uint8_t digest[DIGEST_LEN];
Expand All @@ -19,6 +25,10 @@ static uint8_t dataBuffer[FLASH_PAGE_SIZE]; // holds data for decryption
static uint32_t bufferBase; // address corresponding to base of buffer
static uint32_t bufferStart; // start of encrypted data in buffer
static uint32_t bufferEnd; // length of encrypted data in buffer
static uint32_t startTime;
static uint32_t bytesRead;
static uint8_t progressBuf[5];
static bool digestFailed;

// get a 16 bit word

Expand All @@ -31,58 +41,101 @@ static uint32 getWord16(uint8 *ptr) {
static uint32 getWord32(uint8 *ptr) {
return *ptr + (ptr[1] << 8) + (ptr[2] << 16) + (ptr[3] << 24);
}

// put a 32 bit word

static uint32 putWord32(uint8 *ptr, uint32_t val) {
ptr[0] = (uint8) val;
ptr[1] = (uint8) (val >> 8);
ptr[2] = (uint8) (val >> 16);
ptr[3] = (uint8) (val >> 24);
}

static void decode() {
if (bufferEnd != bufferStart) {
uint8_t newIv[IV_LEN];
// save the last block of ciphertext as the new IV
memcpy(newIv, dataBuffer + bufferEnd - IV_LEN, IV_LEN);
uint8_t * bp = dataBuffer+bufferStart;
CRYPTO_AES_CBC256(CRYPTO, bp, bp, bufferEnd-bufferStart, deKey, iv, false);
uint8_t *bp = dataBuffer + bufferStart;
CRYPTO_AES_CBC256(CRYPTO, bp, bp, bufferEnd - bufferStart, deKey, iv, false);
memcpy(iv, newIv, IV_LEN);
printf("Flashing block at %X\n", bufferBase);
FLASH_eraseOneBlock(bufferBase);
FLASH_writeBlock((void *) bufferBase, FLASH_PAGE_SIZE, dataBuffer);
}
}


// get time since boot in ms
static uint32_t getTime() {
struct gecko_msg_hardware_get_time_rsp_t *tp = gecko_cmd_hardware_get_time();
return tp->seconds * 1000 + tp->ticks / 328;
}

/**
* Set the address of the buffer base. Copy existing data if required.
* @param address The next address to write to
*/
static void setAddress(uint32_t address) {
dataAddress = address;
//printf("Set address to %X\n", address);
uint32_t base = address & ~(FLASH_PAGE_SIZE - 1); // get start of block
if (bufferBase != base) {
decode();
bufferBase = base;
// prefill the buffer with whatever data is already there, in case we want to write a partial block
memcpy(dataBuffer, (const void *) bufferBase, FLASH_PAGE_SIZE);
bufferStart = address-base;
bufferStart = address - base;
bufferEnd = bufferStart;
}
}

void dumphex(const uint8_t * buf, unsigned len) {
while(len-- != 0)
void dumphex(const uint8_t *buf, unsigned len) {
while (len-- != 0)
printf("%02X ", *buf++);
}

bool checkDigest() {
CRYPTO_SHA_256(CRYPTO, (const uint8_t *) digestAddress, digestSize, calcDigest);
if(memcmp(digest, calcDigest, DIGEST_LEN) != 0) {
if (memcmp(digest, calcDigest, DIGEST_LEN) != 0) {
digestFailed = true;
#if defined(DEBUG)
printf("Digest failed: expected:\n");
dumphex(digest, DIGEST_LEN);
printf("\nActually got:\n");
dumphex(calcDigest, DIGEST_LEN);
#endif
progressBuf[0] = DIGEST_FAILED;
gecko_cmd_gatt_server_send_characteristic_notification(currentConnection, GATTDB_ota_progress,
1, progressBuf);
return false;
}
digestFailed = false;
return true;
}

// copy a block of data into the buffer. Side effects include writing it to memory.

static void copydata(uint8_t *packet, uint32_t len) {
uint32_t offs = dataAddress - bufferBase;
memcpy(dataBuffer + offs, packet, len);
dataAddress += len;
bufferEnd = offs + len;
//printf("Length remaining %d\n", count);
if (dataAddress == baseAddress + dataCount) {
uint32_t duration = getTime() - startTime;
printf("Transferred %u bytes in %d.%1d seconds at %d/sec\n", bytesRead, duration / 1000, (duration % 1000) / 10,
bytesRead * 1000 / duration);
decode();
dataCount = 0;
} else
setAddress(dataAddress);
}

// process a data packet.
bool processDataPacket(uint8 *packet, uint8 len) {
//printf("Data packet len %d\n", len);
if (len != 68)
printf("Data packet len %d\n", len);
if (ivLen != 0) {
if (len == ivLen) {
memcpy(iv, packet, len);
Expand All @@ -94,11 +147,11 @@ bool processDataPacket(uint8 *packet, uint8 len) {
return false;
}

if(digestLen != 0) {
if (digestLen != 0) {
if (len == digestLen) {
memcpy(digest, packet, len);
digestLen = 0;
if(checkDigest())
if (checkDigest())
return true;
return false;
}
Expand All @@ -107,17 +160,31 @@ bool processDataPacket(uint8 *packet, uint8 len) {
return false;
}

if (dataCount >= len) {
uint32_t offs = dataAddress-bufferBase;
memcpy(dataBuffer+offs, packet, len);
dataAddress += len;
bufferEnd = offs+len;
dataCount -= len;
//printf("Length remaining %d\n", count);
if(dataCount == 0)
decode();
else
setAddress(dataAddress);
uint32_t baddr = getWord32(packet);
if (baddr != dataAddress) {
printf("packet address %X != expected %X\n", baddr, dataAddress);
if (baddr > dataAddress) {
progressBuf[0] = DFU_RESYNC;
putWord32(progressBuf + 1, dataAddress);
gecko_cmd_gatt_server_send_characteristic_notification(currentConnection, GATTDB_ota_progress,
sizeof(progressBuf), progressBuf);
return true;
}
setAddress(baddr);
}

uint8_t dlen = (uint8_t) (len - 4);
bytesRead += dlen;
if (dlen + dataAddress <= baseAddress + dataCount) {
// does the packet cross a page boundary?
uint32_t offs = dataAddress - bufferBase;
if (bufferEnd + dlen > FLASH_PAGE_SIZE) {
uint32_t tlen = FLASH_PAGE_SIZE - bufferEnd;
copydata(packet + 4, tlen);
dlen -= tlen;
packet += tlen;
}
copydata(packet + 4, dlen);
return true;
}
return false;
Expand All @@ -137,19 +204,26 @@ bool processCtrlPacket(uint8 *packet) {
bufferEnd = 0;
bufferStart = 0;
printf("Restarted DFU\n");
int i = gecko_cmd_le_gap_set_conn_parameters(MIN_CONN_INTERVAL, MAX_CONN_INTERVAL, LATENCY,
SUPERV_TIMEOUT)->result;
if (i != 0)
printf("set_conn_parameters failed: error %u\n", i);
return true;

case DFU_CMD_DATA:
if (ivLen != 0 || dataCount != 0) {
printf("DATA command before previous complete\n");
return false;
}
if(address < (uint32) USER_BLAT) {
if (address < (uint32) USER_BLAT) {
printf("Invalid address - %X should be less than %X", address, USER_BLAT);
return false;
}
dataCount = len;
baseAddress = address;
setAddress(address);
startTime = getTime();
bytesRead = 0;
printf("DATA command: %d bytes at %X\n", len, address);
return true;

Expand Down Expand Up @@ -182,9 +256,9 @@ bool processCtrlPacket(uint8 *packet) {
return true;

case DFU_CMD_DONE:
if (digestLen != 0 || ivLen != 0 || dataCount != 0)
if (digestFailed || digestLen != 0 || ivLen != 0 || dataCount != 0)
return false;
if(USER_BLAT->type == APP_BOOT_ADDRESS_TYPE) {
if (USER_BLAT->type == APP_BOOT_ADDRESS_TYPE) {
printf("Updating BLAT:");
MSC->WRITECTRL |= MSC_WRITECTRL_WREN;
FLASH_writeWord((uint32_t) &USER_BLAT->type, APP_APP_ADDRESS_TYPE);
Expand All @@ -195,8 +269,10 @@ bool processCtrlPacket(uint8 *packet) {

case DFU_CMD_PING:
// checking if we are up to the same point as the master thinks we should be
printf("Pinged at %d/%d\n", len, dataCount);
return len == dataCount;
printf("Pinged at %d/%d\n", len, dataAddress - baseAddress);
uint32_t t = getTime() - startTime;
printf("time: %d.%02d, rate %d/sec\n", t / 1000, (t % 1000) / 10, (bytesRead * 1000) / t);
return true;

default:
break;
Expand Down
Loading

0 comments on commit cc791ab

Please sign in to comment.