This code is inspired by JeanDo ostc-companion.
Signed-off-by: Anton Lundin glance@acc.umu.se --- src/hw_ostc3.c | 120 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 120 insertions(+)
diff --git a/src/hw_ostc3.c b/src/hw_ostc3.c index b78702c..cc1deb3 100644 --- a/src/hw_ostc3.c +++ b/src/hw_ostc3.c @@ -21,6 +21,7 @@
#include <string.h> // memcmp, memcpy #include <stdlib.h> // malloc, free +#include <stdio.h> // FILE, fopen
#include <libdivecomputer/hw_ostc3.h>
@@ -30,6 +31,7 @@ #include "checksum.h" #include "ringbuffer.h" #include "array.h" +#include "aes.h"
#define ISINSTANCE(device) dc_device_isinstance((device), &hw_ostc3_device_vtable)
@@ -43,6 +45,7 @@ #define SZ_VERSION (SZ_CUSTOMTEXT + 4) #define SZ_MEMORY 0x200000 #define SZ_CONFIG 4 +#define SZ_FIRMWARE 0x01E000 // 120KB
#define RB_LOGBOOK_SIZE 256 #define RB_LOGBOOK_COUNT 256 @@ -622,3 +625,120 @@ hw_ostc3_device_config_reset (dc_device_t *abstract)
return DC_STATUS_SUCCESS; } + +typedef struct hw_ostc3_firmware_t { + unsigned char data[SZ_FIRMWARE]; + unsigned int checksum; +} hw_ostc3_firmware_t; + +// FIXME: This key is specific to the ostc3 +// The ostc sport has another key but the same protocoll +// How should we refactor the code for that? +static unsigned char ostc3_key[16] = { + 0xF1, 0xE9, 0xB0, 0x30, + 0x45, 0x6F, 0xBE, 0x55, + 0xFF, 0xE7, 0xF8, 0x31, + 0x13, 0x6C, 0xF2, 0xFE +}; + +static unsigned int hw_ostc3_firmware_checksum(hw_ostc3_firmware_t *firmware) +{ + unsigned short low = 0; + unsigned short high = 0; + for(unsigned int i = 0;i < SZ_FIRMWARE;i++) + { + low += firmware->data[i]; + high += low; + } + return (((unsigned int)high) << 16) + low; +} + +static dc_status_t +hw_ostc3_firmware_readfile (hw_ostc3_firmware_t *firmware, dc_context_t *context, const char *filename) +{ + dc_status_t rc = DC_STATUS_SUCCESS; + FILE *fp; + unsigned char iv[16] = {0}; + unsigned char tmpbuf[16] = {0}; + unsigned char encrypted[16] = {0}; + unsigned int bytes = 0, addr = 0, faddr = 0; + char checksum[4]; + + if (firmware == NULL) { + ERROR (context, "Invalid arguments."); + return DC_STATUS_INVALIDARGS; + } + + // Initialize the buffers. + memset (firmware->data, 0xFF, sizeof (firmware->data)); + firmware->checksum = 0; + + fp = fopen (filename, "rb"); + if (fp == NULL) { + ERROR (context, "Failed to open the file."); + return DC_STATUS_IO; + } + + // FIXME: replace with array_convert_hex2bin + if (fscanf(fp, ":000000" + "%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx" + "%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx" + "\n", + iv, iv + 1, iv + 2, iv + 3, iv + 4, iv + 5, iv + 6, iv + 7, + iv + 8, iv + 9, iv + 10, iv + 11, iv + 12, iv + 13, iv + 14, iv + 15) != 16) { + fclose(fp); + ERROR (context, "Failed to parse header."); + return DC_STATUS_IO; + } + bytes += 16; + + // Load the iv for AES-FCB-mode + AES128_ECB_encrypt(iv, ostc3_key, tmpbuf); + + for(addr = 0; addr < SZ_FIRMWARE; addr += 16) { + // FIXME: replace with array_convert_hex2bin + if (fscanf(fp, ":%6x" + "%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx" + "%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx%2hhx" + "\n", + &faddr, + encrypted, encrypted + 1, encrypted + 2, encrypted + 3, encrypted + 4, encrypted + 5, encrypted + 6, encrypted + 7, + encrypted + 8, encrypted + 9, encrypted + 10, encrypted + 11, encrypted + 12, encrypted + 13, encrypted + 14, encrypted + 15) != 17) { + fclose(fp); + ERROR (context, "Failed to parse data."); + return DC_STATUS_IO; + } + + if (faddr != bytes) { + fclose(fp); + ERROR (context, "Failed to data address."); + return DC_STATUS_IO; + } + + // Decrypt AES-FCB data + for(int i=0;i < 16;i++) + firmware->data[addr + i] = encrypted[i] ^ tmpbuf[i]; + + // Run the next round of encryption + AES128_ECB_encrypt(encrypted, ostc3_key, tmpbuf); + + bytes += 16; + } + + // This file format contains a tail with the checksum in + if (fscanf(fp, ":%6x%2hhx%2hhx%2hhx%2hhx\n", &faddr, checksum, checksum + 1, checksum + 2, checksum + 3) != 5 || faddr != bytes) { + fclose(fp); + ERROR (context, "Failed to parse file tail."); + return DC_STATUS_IO; + } + fclose(fp); + + firmware->checksum = array_uint32_le(checksum); + + if (firmware->checksum != hw_ostc3_firmware_checksum(firmware)) { + ERROR (context, "Failed to verify file checksum."); + return DC_STATUS_IO; + } + + return DC_STATUS_SUCCESS; +}