This code is inspired by JeanDo ostc-companion.
Reviewed-by: Jef Driesen jef@libdivecomputer.org Signed-off-by: Anton Lundin glance@acc.umu.se --- src/hw_ostc3.c | 136 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 136 insertions(+)
diff --git a/src/hw_ostc3.c b/src/hw_ostc3.c index b78702c..a4885d4 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 @@ -66,6 +69,21 @@ typedef struct hw_ostc3_device_t { unsigned char fingerprint[5]; } hw_ostc3_device_t;
+typedef struct hw_ostc3_firmware_t { + unsigned char data[SZ_FIRMWARE]; + unsigned int checksum; +} hw_ostc3_firmware_t; + +// This key is used both for the Ostc3 and its cousin, +// the Ostc Sport. +// The Frog uses a similar protocol, and with another key. +static const unsigned char ostc3_key[16] = { + 0xF1, 0xE9, 0xB0, 0x30, + 0x45, 0x6F, 0xBE, 0x55, + 0xFF, 0xE7, 0xF8, 0x31, + 0x13, 0x6C, 0xF2, 0xFE +}; + static dc_status_t hw_ostc3_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size); static dc_status_t hw_ostc3_device_foreach (dc_device_t *abstract, dc_dive_callback_t callback, void *userdata); static dc_status_t hw_ostc3_device_close (dc_device_t *abstract); @@ -622,3 +640,121 @@ hw_ostc3_device_config_reset (dc_device_t *abstract)
return DC_STATUS_SUCCESS; } + +// This is a variant of fletcher16 with 16 bit sum instead of 8bit +// and modulo 256 instead of 255 +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_readline(FILE *fp, unsigned int addr, unsigned char data[], unsigned int size) +{ + unsigned char ascii[40]; + // 1 byte :, 6 bytes addr, X*2 bytes hex -> X bytes data. + const unsigned line_size = size * 2 + 1 + 6 + 1; + unsigned char faddr_byte[3]; + unsigned int faddr = 0; + if (line_size > sizeof(ascii)) + return DC_STATUS_INVALIDARGS; + if (fread (ascii, sizeof(char), line_size, fp) != line_size) + return DC_STATUS_IO; + if (ascii[0] != ':') + return DC_STATUS_DATAFORMAT; + // Is it a CRLF file? + // Read away that trailing LF + if (ascii[line_size] == '\r') + if (fread (ascii + line_size, sizeof(char), 1, fp) != 1) + return DC_STATUS_DATAFORMAT; + if (array_convert_hex2bin(ascii + 1, 6, faddr_byte, sizeof(faddr_byte)) != 0) { + return DC_STATUS_DATAFORMAT; + } + faddr = array_uint24_be (faddr_byte); + if (faddr != addr) + return DC_STATUS_DATAFORMAT; + if (array_convert_hex2bin (ascii + 1 + 6, size*2, data, size) != 0) + return DC_STATUS_DATAFORMAT; + + return DC_STATUS_SUCCESS; +} + + +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; + unsigned 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; + } + + rc = hw_ostc3_firmware_readline(fp, 0, iv, sizeof(iv)); + if (rc != DC_STATUS_SUCCESS) { + fclose (fp); + ERROR (context, "Failed to parse header."); + return rc; + } + bytes += 16; + + // Load the iv for AES-FCB-mode + AES128_ECB_encrypt(iv, ostc3_key, tmpbuf); + + for(addr = 0; addr < SZ_FIRMWARE; addr += 16, bytes += 16) { + rc = hw_ostc3_firmware_readline(fp, bytes, encrypted, sizeof(encrypted)); + if (rc != DC_STATUS_SUCCESS) { + fclose (fp); + ERROR (context, "Failed to parse file data."); + return rc; + } + + // 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); + } + + // This file format contains a tail with the checksum in + rc = hw_ostc3_firmware_readline(fp, bytes, checksum, sizeof(checksum)); + if (rc != DC_STATUS_SUCCESS) { + fclose (fp); + ERROR (context, "Failed to parse file tail."); + return rc; + } + 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; +}