diff --git a/examples/hw_ostc_fwupdate.c b/examples/hw_ostc_fwupdate.c index a9e7fde..8fa5c31 100644 --- a/examples/hw_ostc_fwupdate.c +++ b/examples/hw_ostc_fwupdate.c @@ -19,10 +19,10 @@ * MA 02110-1301 USA */ -#include // fopen, fwrite, fclose -#include +#include #include +#include #include "utils.h" #include "common.h" @@ -44,17 +44,23 @@ event_cb (dc_device_t *device, dc_event_type_t event, const void *data, void *us } static dc_status_t -fwupdate (const char *name, const char *hexfile) +fwupdate (const char *name, const char *hexfile, int ostc3) { dc_context_t *context = NULL; dc_device_t *device = NULL; + dc_status_t rc = DC_STATUS_SUCCESS; dc_context_new (&context); dc_context_set_loglevel (context, DC_LOGLEVEL_ALL); dc_context_set_logfunc (context, logfunc, NULL); - message ("hw_ostc_device_open\n"); - dc_status_t rc = hw_ostc_device_open (&device, context, name); + if (ostc3) { + message ("hw_ostc3_device_open\n"); + rc = hw_ostc3_device_open (&device, context, name); + } else { + message ("hw_ostc_device_open\n"); + rc = hw_ostc_device_open (&device, context, name); + } if (rc != DC_STATUS_SUCCESS) { WARNING ("Error opening serial port."); dc_context_free (context); @@ -70,8 +76,13 @@ fwupdate (const char *name, const char *hexfile) return rc; } - message ("hw_ostc_device_fwupdate\n"); - rc = hw_ostc_device_fwupdate (device, hexfile); + if (ostc3) { + message ("hw_ostc3_device_fwupdate\n"); + rc = hw_ostc3_device_fwupdate (device, hexfile); + } else { + message ("hw_ostc_device_fwupdate\n"); + rc = hw_ostc_device_fwupdate (device, hexfile); + } if (rc != DC_STATUS_SUCCESS) { WARNING ("Error flashing firmware."); dc_device_close (device); @@ -103,6 +114,7 @@ int main(int argc, char *argv[]) const char* name = "/dev/ttyUSB0"; #endif const char *hexfile = NULL; + int ostc3 = 0; if (argc > 1) { name = argv[1]; @@ -110,11 +122,18 @@ int main(int argc, char *argv[]) if (argc > 2) { hexfile = argv[2]; } + if (argc > 3) { + if (strcmp(argv[3], "-3") == 0) { + ostc3 = 1; + } else { + ostc3 = 0; + } + } message ("DEVICE=%s\n", name); message ("HEXFILE=%s\n", hexfile); - dc_status_t a = fwupdate (name, hexfile); + dc_status_t a = fwupdate (name, hexfile, ostc3); message ("SUMMARY\n"); message ("-------\n"); diff --git a/include/libdivecomputer/hw_ostc3.h b/include/libdivecomputer/hw_ostc3.h index 267b7e3..bc56a9d 100644 --- a/include/libdivecomputer/hw_ostc3.h +++ b/include/libdivecomputer/hw_ostc3.h @@ -58,6 +58,9 @@ hw_ostc3_device_config_write (dc_device_t *abstract, unsigned int config, const dc_status_t hw_ostc3_device_config_reset (dc_device_t *abstract); +dc_status_t +hw_ostc3_device_fwupdate (dc_device_t *abstract, const char *filename); + #ifdef __cplusplus } #endif /* __cplusplus */ diff --git a/msvc/libdivecomputer.vcproj b/msvc/libdivecomputer.vcproj index 2fea2b9..4f8a958 100644 --- a/msvc/libdivecomputer.vcproj +++ b/msvc/libdivecomputer.vcproj @@ -178,6 +178,10 @@ Filter="cpp;c;cc;cxx;def;odl;idl;hpj;bat;asm;asmx" UniqueIdentifier="{4FC737F1-C7A5-4376-A066-2A32D752A2FF}" > + + @@ -476,6 +480,10 @@ Filter="h;hpp;hxx;hm;inl;inc;xsd" UniqueIdentifier="{93995380-89BD-4b04-88EB-625FBE52EBFB}" > + + diff --git a/src/Makefile.am b/src/Makefile.am index 1f52841..821bc1b 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -44,6 +44,7 @@ libdivecomputer_la_SOURCES = \ ihex.h ihex.c \ hw_ostc.c hw_ostc_parser.c \ hw_frog.c \ + aes.h aes.c \ hw_ostc3.c \ cressi_edy.c cressi_edy_parser.c \ cressi_leonardo.c cressi_leonardo_parser.c \ diff --git a/src/aes.c b/src/aes.c new file mode 100644 index 0000000..a2f00dd --- /dev/null +++ b/src/aes.c @@ -0,0 +1,589 @@ +/* + +This is an implementation of the AES128 algorithm, specifically ECB and CBC mode. + +The implementation is verified against the test vectors in: + National Institute of Standards and Technology Special Publication 800-38A 2001 ED + +ECB-AES128 +---------- + + plain-text: + 6bc1bee22e409f96e93d7e117393172a + ae2d8a571e03ac9c9eb76fac45af8e51 + 30c81c46a35ce411e5fbc1191a0a52ef + f69f2445df4f9b17ad2b417be66c3710 + + key: + 2b7e151628aed2a6abf7158809cf4f3c + + resulting cipher + 3ad77bb40d7a3660a89ecaf32466ef97 + f5d3d58503b9699de785895a96fdbaaf + 43b1cd7f598ece23881b00e3ed030688 + 7b0c785e27e8ad3f8223207104725dd4 + + +NOTE: String length must be evenly divisible by 16byte (str_len % 16 == 0) + You should pad the end of the string with zeros if this is not the case. + +*/ + + +/*****************************************************************************/ +/* Includes: */ +/*****************************************************************************/ +#include // CBC mode, for memset +#include "aes.h" + + +/*****************************************************************************/ +/* Defines: */ +/*****************************************************************************/ +// The number of columns comprising a state in AES. This is a constant in AES. Value=4 +#define Nb 4 +// The number of 32 bit words in a key. +#define Nk 4 +// Key length in bytes [128 bit] +#define KEYLEN 16 +// The number of rounds in AES Cipher. +#define Nr 10 + +// jcallan@github points out that declaring Multiply as a function +// reduces code size considerably with the Keil ARM compiler. +// See this link for more information: https://github.com/kokke/tiny-AES128-C/pull/3 +#ifndef MULTIPLY_AS_A_FUNCTION + #define MULTIPLY_AS_A_FUNCTION 0 +#endif + + +/*****************************************************************************/ +/* Private variables: */ +/*****************************************************************************/ +// state - array holding the intermediate results during decryption. +typedef uint8_t state_t[4][4]; + +typedef struct aes_state_t { + state_t* state; + + // The array that stores the round keys. + uint8_t RoundKey[176]; + + // The Key input to the AES Program + const uint8_t* Key; + +#if defined(CBC) && CBC + // Initial Vector used only for CBC mode + uint8_t* Iv; +#endif +} aes_state_t; + +// The lookup-tables are marked const so they can be placed in read-only storage instead of RAM +// The numbers below can be computed dynamically trading ROM for RAM - +// This can be useful in (embedded) bootloader applications, where ROM is often limited. +static const uint8_t sbox[256] = { + //0 1 2 3 4 5 6 7 8 9 A B C D E F + 0x63, 0x7c, 0x77, 0x7b, 0xf2, 0x6b, 0x6f, 0xc5, 0x30, 0x01, 0x67, 0x2b, 0xfe, 0xd7, 0xab, 0x76, + 0xca, 0x82, 0xc9, 0x7d, 0xfa, 0x59, 0x47, 0xf0, 0xad, 0xd4, 0xa2, 0xaf, 0x9c, 0xa4, 0x72, 0xc0, + 0xb7, 0xfd, 0x93, 0x26, 0x36, 0x3f, 0xf7, 0xcc, 0x34, 0xa5, 0xe5, 0xf1, 0x71, 0xd8, 0x31, 0x15, + 0x04, 0xc7, 0x23, 0xc3, 0x18, 0x96, 0x05, 0x9a, 0x07, 0x12, 0x80, 0xe2, 0xeb, 0x27, 0xb2, 0x75, + 0x09, 0x83, 0x2c, 0x1a, 0x1b, 0x6e, 0x5a, 0xa0, 0x52, 0x3b, 0xd6, 0xb3, 0x29, 0xe3, 0x2f, 0x84, + 0x53, 0xd1, 0x00, 0xed, 0x20, 0xfc, 0xb1, 0x5b, 0x6a, 0xcb, 0xbe, 0x39, 0x4a, 0x4c, 0x58, 0xcf, + 0xd0, 0xef, 0xaa, 0xfb, 0x43, 0x4d, 0x33, 0x85, 0x45, 0xf9, 0x02, 0x7f, 0x50, 0x3c, 0x9f, 0xa8, + 0x51, 0xa3, 0x40, 0x8f, 0x92, 0x9d, 0x38, 0xf5, 0xbc, 0xb6, 0xda, 0x21, 0x10, 0xff, 0xf3, 0xd2, + 0xcd, 0x0c, 0x13, 0xec, 0x5f, 0x97, 0x44, 0x17, 0xc4, 0xa7, 0x7e, 0x3d, 0x64, 0x5d, 0x19, 0x73, + 0x60, 0x81, 0x4f, 0xdc, 0x22, 0x2a, 0x90, 0x88, 0x46, 0xee, 0xb8, 0x14, 0xde, 0x5e, 0x0b, 0xdb, + 0xe0, 0x32, 0x3a, 0x0a, 0x49, 0x06, 0x24, 0x5c, 0xc2, 0xd3, 0xac, 0x62, 0x91, 0x95, 0xe4, 0x79, + 0xe7, 0xc8, 0x37, 0x6d, 0x8d, 0xd5, 0x4e, 0xa9, 0x6c, 0x56, 0xf4, 0xea, 0x65, 0x7a, 0xae, 0x08, + 0xba, 0x78, 0x25, 0x2e, 0x1c, 0xa6, 0xb4, 0xc6, 0xe8, 0xdd, 0x74, 0x1f, 0x4b, 0xbd, 0x8b, 0x8a, + 0x70, 0x3e, 0xb5, 0x66, 0x48, 0x03, 0xf6, 0x0e, 0x61, 0x35, 0x57, 0xb9, 0x86, 0xc1, 0x1d, 0x9e, + 0xe1, 0xf8, 0x98, 0x11, 0x69, 0xd9, 0x8e, 0x94, 0x9b, 0x1e, 0x87, 0xe9, 0xce, 0x55, 0x28, 0xdf, + 0x8c, 0xa1, 0x89, 0x0d, 0xbf, 0xe6, 0x42, 0x68, 0x41, 0x99, 0x2d, 0x0f, 0xb0, 0x54, 0xbb, 0x16 }; + +static const uint8_t rsbox[256] = +{ 0x52, 0x09, 0x6a, 0xd5, 0x30, 0x36, 0xa5, 0x38, 0xbf, 0x40, 0xa3, 0x9e, 0x81, 0xf3, 0xd7, 0xfb, + 0x7c, 0xe3, 0x39, 0x82, 0x9b, 0x2f, 0xff, 0x87, 0x34, 0x8e, 0x43, 0x44, 0xc4, 0xde, 0xe9, 0xcb, + 0x54, 0x7b, 0x94, 0x32, 0xa6, 0xc2, 0x23, 0x3d, 0xee, 0x4c, 0x95, 0x0b, 0x42, 0xfa, 0xc3, 0x4e, + 0x08, 0x2e, 0xa1, 0x66, 0x28, 0xd9, 0x24, 0xb2, 0x76, 0x5b, 0xa2, 0x49, 0x6d, 0x8b, 0xd1, 0x25, + 0x72, 0xf8, 0xf6, 0x64, 0x86, 0x68, 0x98, 0x16, 0xd4, 0xa4, 0x5c, 0xcc, 0x5d, 0x65, 0xb6, 0x92, + 0x6c, 0x70, 0x48, 0x50, 0xfd, 0xed, 0xb9, 0xda, 0x5e, 0x15, 0x46, 0x57, 0xa7, 0x8d, 0x9d, 0x84, + 0x90, 0xd8, 0xab, 0x00, 0x8c, 0xbc, 0xd3, 0x0a, 0xf7, 0xe4, 0x58, 0x05, 0xb8, 0xb3, 0x45, 0x06, + 0xd0, 0x2c, 0x1e, 0x8f, 0xca, 0x3f, 0x0f, 0x02, 0xc1, 0xaf, 0xbd, 0x03, 0x01, 0x13, 0x8a, 0x6b, + 0x3a, 0x91, 0x11, 0x41, 0x4f, 0x67, 0xdc, 0xea, 0x97, 0xf2, 0xcf, 0xce, 0xf0, 0xb4, 0xe6, 0x73, + 0x96, 0xac, 0x74, 0x22, 0xe7, 0xad, 0x35, 0x85, 0xe2, 0xf9, 0x37, 0xe8, 0x1c, 0x75, 0xdf, 0x6e, + 0x47, 0xf1, 0x1a, 0x71, 0x1d, 0x29, 0xc5, 0x89, 0x6f, 0xb7, 0x62, 0x0e, 0xaa, 0x18, 0xbe, 0x1b, + 0xfc, 0x56, 0x3e, 0x4b, 0xc6, 0xd2, 0x79, 0x20, 0x9a, 0xdb, 0xc0, 0xfe, 0x78, 0xcd, 0x5a, 0xf4, + 0x1f, 0xdd, 0xa8, 0x33, 0x88, 0x07, 0xc7, 0x31, 0xb1, 0x12, 0x10, 0x59, 0x27, 0x80, 0xec, 0x5f, + 0x60, 0x51, 0x7f, 0xa9, 0x19, 0xb5, 0x4a, 0x0d, 0x2d, 0xe5, 0x7a, 0x9f, 0x93, 0xc9, 0x9c, 0xef, + 0xa0, 0xe0, 0x3b, 0x4d, 0xae, 0x2a, 0xf5, 0xb0, 0xc8, 0xeb, 0xbb, 0x3c, 0x83, 0x53, 0x99, 0x61, + 0x17, 0x2b, 0x04, 0x7e, 0xba, 0x77, 0xd6, 0x26, 0xe1, 0x69, 0x14, 0x63, 0x55, 0x21, 0x0c, 0x7d }; + + +// The round constant word array, Rcon[i], contains the values given by +// x to th e power (i-1) being powers of x (x is denoted as {02}) in the field GF(2^8) +// Note that i starts at 1, not 0). +static const uint8_t Rcon[255] = { + 0x8d, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36, 0x6c, 0xd8, 0xab, 0x4d, 0x9a, + 0x2f, 0x5e, 0xbc, 0x63, 0xc6, 0x97, 0x35, 0x6a, 0xd4, 0xb3, 0x7d, 0xfa, 0xef, 0xc5, 0x91, 0x39, + 0x72, 0xe4, 0xd3, 0xbd, 0x61, 0xc2, 0x9f, 0x25, 0x4a, 0x94, 0x33, 0x66, 0xcc, 0x83, 0x1d, 0x3a, + 0x74, 0xe8, 0xcb, 0x8d, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36, 0x6c, 0xd8, + 0xab, 0x4d, 0x9a, 0x2f, 0x5e, 0xbc, 0x63, 0xc6, 0x97, 0x35, 0x6a, 0xd4, 0xb3, 0x7d, 0xfa, 0xef, + 0xc5, 0x91, 0x39, 0x72, 0xe4, 0xd3, 0xbd, 0x61, 0xc2, 0x9f, 0x25, 0x4a, 0x94, 0x33, 0x66, 0xcc, + 0x83, 0x1d, 0x3a, 0x74, 0xe8, 0xcb, 0x8d, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, + 0x36, 0x6c, 0xd8, 0xab, 0x4d, 0x9a, 0x2f, 0x5e, 0xbc, 0x63, 0xc6, 0x97, 0x35, 0x6a, 0xd4, 0xb3, + 0x7d, 0xfa, 0xef, 0xc5, 0x91, 0x39, 0x72, 0xe4, 0xd3, 0xbd, 0x61, 0xc2, 0x9f, 0x25, 0x4a, 0x94, + 0x33, 0x66, 0xcc, 0x83, 0x1d, 0x3a, 0x74, 0xe8, 0xcb, 0x8d, 0x01, 0x02, 0x04, 0x08, 0x10, 0x20, + 0x40, 0x80, 0x1b, 0x36, 0x6c, 0xd8, 0xab, 0x4d, 0x9a, 0x2f, 0x5e, 0xbc, 0x63, 0xc6, 0x97, 0x35, + 0x6a, 0xd4, 0xb3, 0x7d, 0xfa, 0xef, 0xc5, 0x91, 0x39, 0x72, 0xe4, 0xd3, 0xbd, 0x61, 0xc2, 0x9f, + 0x25, 0x4a, 0x94, 0x33, 0x66, 0xcc, 0x83, 0x1d, 0x3a, 0x74, 0xe8, 0xcb, 0x8d, 0x01, 0x02, 0x04, + 0x08, 0x10, 0x20, 0x40, 0x80, 0x1b, 0x36, 0x6c, 0xd8, 0xab, 0x4d, 0x9a, 0x2f, 0x5e, 0xbc, 0x63, + 0xc6, 0x97, 0x35, 0x6a, 0xd4, 0xb3, 0x7d, 0xfa, 0xef, 0xc5, 0x91, 0x39, 0x72, 0xe4, 0xd3, 0xbd, + 0x61, 0xc2, 0x9f, 0x25, 0x4a, 0x94, 0x33, 0x66, 0xcc, 0x83, 0x1d, 0x3a, 0x74, 0xe8, 0xcb }; + + +/*****************************************************************************/ +/* Private functions: */ +/*****************************************************************************/ +static uint8_t getSBoxValue(uint8_t num) +{ + return sbox[num]; +} + +static uint8_t getSBoxInvert(uint8_t num) +{ + return rsbox[num]; +} + +// This function produces Nb(Nr+1) round keys. The round keys are used in each round to decrypt the states. +static void KeyExpansion(aes_state_t *state) +{ + uint32_t i, j, k; + uint8_t tempa[4]; // Used for the column/row operations + + // The first round key is the key itself. + for(i = 0; i < Nk; ++i) + { + state->RoundKey[(i * 4) + 0] = state->Key[(i * 4) + 0]; + state->RoundKey[(i * 4) + 1] = state->Key[(i * 4) + 1]; + state->RoundKey[(i * 4) + 2] = state->Key[(i * 4) + 2]; + state->RoundKey[(i * 4) + 3] = state->Key[(i * 4) + 3]; + } + + // All other round keys are found from the previous round keys. + for(; (i < (Nb * (Nr + 1))); ++i) + { + for(j = 0; j < 4; ++j) + { + tempa[j]=state->RoundKey[(i-1) * 4 + j]; + } + if (i % Nk == 0) + { + // This function rotates the 4 bytes in a word to the left once. + // [a0,a1,a2,a3] becomes [a1,a2,a3,a0] + + // Function RotWord() + { + k = tempa[0]; + tempa[0] = tempa[1]; + tempa[1] = tempa[2]; + tempa[2] = tempa[3]; + tempa[3] = k; + } + + // SubWord() is a function that takes a four-byte input word and + // applies the S-box to each of the four bytes to produce an output word. + + // Function Subword() + { + tempa[0] = getSBoxValue(tempa[0]); + tempa[1] = getSBoxValue(tempa[1]); + tempa[2] = getSBoxValue(tempa[2]); + tempa[3] = getSBoxValue(tempa[3]); + } + + tempa[0] = tempa[0] ^ Rcon[i/Nk]; + } + else if (Nk > 6 && i % Nk == 4) + { + // Function Subword() + { + tempa[0] = getSBoxValue(tempa[0]); + tempa[1] = getSBoxValue(tempa[1]); + tempa[2] = getSBoxValue(tempa[2]); + tempa[3] = getSBoxValue(tempa[3]); + } + } + state->RoundKey[i * 4 + 0] = state->RoundKey[(i - Nk) * 4 + 0] ^ tempa[0]; + state->RoundKey[i * 4 + 1] = state->RoundKey[(i - Nk) * 4 + 1] ^ tempa[1]; + state->RoundKey[i * 4 + 2] = state->RoundKey[(i - Nk) * 4 + 2] ^ tempa[2]; + state->RoundKey[i * 4 + 3] = state->RoundKey[(i - Nk) * 4 + 3] ^ tempa[3]; + } +} + +// This function adds the round key to state. +// The round key is added to the state by an XOR function. +static void AddRoundKey(aes_state_t *state, uint8_t round) +{ + uint8_t i,j; + for(i=0;i<4;++i) + { + for(j = 0; j < 4; ++j) + { + (*state->state)[i][j] ^= state->RoundKey[round * Nb * 4 + i * Nb + j]; + } + } +} + +// The SubBytes Function Substitutes the values in the +// state matrix with values in an S-box. +static void SubBytes(aes_state_t *state) +{ + uint8_t i, j; + for(i = 0; i < 4; ++i) + { + for(j = 0; j < 4; ++j) + { + (*state->state)[j][i] = getSBoxValue((*state->state)[j][i]); + } + } +} + +// The ShiftRows() function shifts the rows in the state to the left. +// Each row is shifted with different offset. +// Offset = Row number. So the first row is not shifted. +static void ShiftRows(aes_state_t *state) +{ + uint8_t temp; + + // Rotate first row 1 columns to left + temp = (*state->state)[0][1]; + (*state->state)[0][1] = (*state->state)[1][1]; + (*state->state)[1][1] = (*state->state)[2][1]; + (*state->state)[2][1] = (*state->state)[3][1]; + (*state->state)[3][1] = temp; + + // Rotate second row 2 columns to left + temp = (*state->state)[0][2]; + (*state->state)[0][2] = (*state->state)[2][2]; + (*state->state)[2][2] = temp; + + temp = (*state->state)[1][2]; + (*state->state)[1][2] = (*state->state)[3][2]; + (*state->state)[3][2] = temp; + + // Rotate third row 3 columns to left + temp = (*state->state)[0][3]; + (*state->state)[0][3] = (*state->state)[3][3]; + (*state->state)[3][3] = (*state->state)[2][3]; + (*state->state)[2][3] = (*state->state)[1][3]; + (*state->state)[1][3] = temp; +} + +static uint8_t xtime(uint8_t x) +{ + return ((x<<1) ^ (((x>>7) & 1) * 0x1b)); +} + +// MixColumns function mixes the columns of the state matrix +static void MixColumns(aes_state_t *state) +{ + uint8_t i; + uint8_t Tmp,Tm,t; + for(i = 0; i < 4; ++i) + { + t = (*state->state)[i][0]; + Tmp = (*state->state)[i][0] ^ (*state->state)[i][1] ^ (*state->state)[i][2] ^ (*state->state)[i][3] ; + Tm = (*state->state)[i][0] ^ (*state->state)[i][1] ; Tm = xtime(Tm); (*state->state)[i][0] ^= Tm ^ Tmp ; + Tm = (*state->state)[i][1] ^ (*state->state)[i][2] ; Tm = xtime(Tm); (*state->state)[i][1] ^= Tm ^ Tmp ; + Tm = (*state->state)[i][2] ^ (*state->state)[i][3] ; Tm = xtime(Tm); (*state->state)[i][2] ^= Tm ^ Tmp ; + Tm = (*state->state)[i][3] ^ t ; Tm = xtime(Tm); (*state->state)[i][3] ^= Tm ^ Tmp ; + } +} + +// Multiply is used to multiply numbers in the field GF(2^8) +#if MULTIPLY_AS_A_FUNCTION +static uint8_t Multiply(uint8_t x, uint8_t y) +{ + return (((y & 1) * x) ^ + ((y>>1 & 1) * xtime(x)) ^ + ((y>>2 & 1) * xtime(xtime(x))) ^ + ((y>>3 & 1) * xtime(xtime(xtime(x)))) ^ + ((y>>4 & 1) * xtime(xtime(xtime(xtime(x)))))); + } +#else +#define Multiply(x, y) \ + ( ((y & 1) * x) ^ \ + ((y>>1 & 1) * xtime(x)) ^ \ + ((y>>2 & 1) * xtime(xtime(x))) ^ \ + ((y>>3 & 1) * xtime(xtime(xtime(x)))) ^ \ + ((y>>4 & 1) * xtime(xtime(xtime(xtime(x)))))) \ + +#endif + +// MixColumns function mixes the columns of the state matrix. +// The method used to multiply may be difficult to understand for the inexperienced. +// Please use the references to gain more information. +static void InvMixColumns(aes_state_t *state) +{ + int i; + uint8_t a,b,c,d; + for(i=0;i<4;++i) + { + a = (*state->state)[i][0]; + b = (*state->state)[i][1]; + c = (*state->state)[i][2]; + d = (*state->state)[i][3]; + + (*state->state)[i][0] = Multiply(a, 0x0e) ^ Multiply(b, 0x0b) ^ Multiply(c, 0x0d) ^ Multiply(d, 0x09); + (*state->state)[i][1] = Multiply(a, 0x09) ^ Multiply(b, 0x0e) ^ Multiply(c, 0x0b) ^ Multiply(d, 0x0d); + (*state->state)[i][2] = Multiply(a, 0x0d) ^ Multiply(b, 0x09) ^ Multiply(c, 0x0e) ^ Multiply(d, 0x0b); + (*state->state)[i][3] = Multiply(a, 0x0b) ^ Multiply(b, 0x0d) ^ Multiply(c, 0x09) ^ Multiply(d, 0x0e); + } +} + + +// The SubBytes Function Substitutes the values in the +// state matrix with values in an S-box. +static void InvSubBytes(aes_state_t *state) +{ + uint8_t i,j; + for(i=0;i<4;++i) + { + for(j=0;j<4;++j) + { + (*state->state)[j][i] = getSBoxInvert((*state->state)[j][i]); + } + } +} + +static void InvShiftRows(aes_state_t *state) +{ + uint8_t temp; + + // Rotate first row 1 columns to right + temp=(*state->state)[3][1]; + (*state->state)[3][1]=(*state->state)[2][1]; + (*state->state)[2][1]=(*state->state)[1][1]; + (*state->state)[1][1]=(*state->state)[0][1]; + (*state->state)[0][1]=temp; + + // Rotate second row 2 columns to right + temp=(*state->state)[0][2]; + (*state->state)[0][2]=(*state->state)[2][2]; + (*state->state)[2][2]=temp; + + temp=(*state->state)[1][2]; + (*state->state)[1][2]=(*state->state)[3][2]; + (*state->state)[3][2]=temp; + + // Rotate third row 3 columns to right + temp=(*state->state)[0][3]; + (*state->state)[0][3]=(*state->state)[1][3]; + (*state->state)[1][3]=(*state->state)[2][3]; + (*state->state)[2][3]=(*state->state)[3][3]; + (*state->state)[3][3]=temp; +} + + +// Cipher is the main function that encrypts the PlainText. +static void Cipher(aes_state_t *state) +{ + uint8_t round = 0; + + // Add the First round key to the state before starting the rounds. + AddRoundKey(state, 0); + + // There will be Nr rounds. + // The first Nr-1 rounds are identical. + // These Nr-1 rounds are executed in the loop below. + for(round = 1; round < Nr; ++round) + { + SubBytes(state); + ShiftRows(state); + MixColumns(state); + AddRoundKey(state, round); + } + + // The last round is given below. + // The MixColumns function is not here in the last round. + SubBytes(state); + ShiftRows(state); + AddRoundKey(state, Nr); +} + +static void InvCipher(aes_state_t *state) +{ + uint8_t round=0; + + // Add the First round key to the state before starting the rounds. + AddRoundKey(state, Nr); + + // There will be Nr rounds. + // The first Nr-1 rounds are identical. + // These Nr-1 rounds are executed in the loop below. + for(round=Nr-1;round>0;round--) + { + InvShiftRows(state); + InvSubBytes(state); + AddRoundKey(state, round); + InvMixColumns(state); + } + + // The last round is given below. + // The MixColumns function is not here in the last round. + InvShiftRows(state); + InvSubBytes(state); + AddRoundKey(state, 0); +} + +static void BlockCopy(uint8_t* output, uint8_t* input) +{ + uint8_t i; + for (i=0;iIv[i]; + } +} + +void AES128_CBC_encrypt_buffer(uint8_t* output, uint8_t* input, uint32_t length, const uint8_t* key, const uint8_t* iv) +{ + intptr_t i; + uint8_t remainders = length % KEYLEN; /* Remaining bytes in the last non-full block */ + aes_state_t state; + + BlockCopy(output, input); + state.state = (state_t*)output; + + // Skip the key expansion if key is passed as 0 + if(0 != key) + { + state.Key = key; + KeyExpansion(&state); + } + + if(iv != 0) + { + state.Iv = (uint8_t*)iv; + } + + for(i = 0; i < length; i += KEYLEN) + { + XorWithIv(&state, input); + BlockCopy(output, input); + state.state = (state_t*)output; + Cipher(&state); + state.Iv = output; + input += KEYLEN; + output += KEYLEN; + } + + if(remainders) + { + BlockCopy(output, input); + memset(output + remainders, 0, KEYLEN - remainders); /* add 0-padding */ + state.state = (state_t*)output; + Cipher(&state); + } +} + +void AES128_CBC_decrypt_buffer(uint8_t* output, uint8_t* input, uint32_t length, const uint8_t* key, const uint8_t* iv) +{ + intptr_t i; + uint8_t remainders = length % KEYLEN; /* Remaining bytes in the last non-full block */ + aes_state_t state; + + BlockCopy(output, input); + state.state = (state_t*)output; + + // Skip the key expansion if key is passed as 0 + if(0 != key) + { + state.Key = key; + KeyExpansion(&state); + } + + // If iv is passed as 0, we continue to encrypt without re-setting the Iv + if(iv != 0) + { + state.Iv = (uint8_t*)iv; + } + + for(i = 0; i < length; i += KEYLEN) + { + BlockCopy(output, input); + state.state = (state_t*)output; + InvCipher(&state); + XorWithIv(&state, output); + state.Iv = input; + input += KEYLEN; + output += KEYLEN; + } + + if(remainders) + { + BlockCopy(output, input); + memset(output+remainders, 0, KEYLEN - remainders); /* add 0-padding */ + state.state = (state_t*)output; + InvCipher(&state); + } +} + + +#endif // #if defined(CBC) && CBC + + diff --git a/src/aes.h b/src/aes.h new file mode 100644 index 0000000..051d813 --- /dev/null +++ b/src/aes.h @@ -0,0 +1,45 @@ +#ifndef _AES_H_ +#define _AES_H_ + +#ifdef _MSC_VER +typedef unsigned char uint8_t; +typedef unsigned int uint32_t; +#else +#include +#endif + + +// #define the macros below to 1/0 to enable/disable the mode of operation. +// +// CBC enables AES128 encryption in CBC-mode of operation and handles 0-padding. +// ECB enables the basic ECB 16-byte block algorithm. Both can be enabled simultaneously. + +// The #ifndef-guard allows it to be configured before #include'ing or at compile time. +#ifndef CBC + #define CBC 1 +#endif + +#ifndef ECB + #define ECB 1 +#endif + + + +#if defined(ECB) && ECB + +void AES128_ECB_encrypt(uint8_t* input, const uint8_t* key, uint8_t *output); +void AES128_ECB_decrypt(uint8_t* input, const uint8_t* key, uint8_t *output); + +#endif // #if defined(ECB) && ECB + + +#if defined(CBC) && CBC + +void AES128_CBC_encrypt_buffer(uint8_t* output, uint8_t* input, uint32_t length, const uint8_t* key, const uint8_t* iv); +void AES128_CBC_decrypt_buffer(uint8_t* output, uint8_t* input, uint32_t length, const uint8_t* key, const uint8_t* iv); + +#endif // #if defined(CBC) && CBC + + + +#endif //_AES_H_ diff --git a/src/array.c b/src/array.c index 243ec34..be8d696 100644 --- a/src/array.c +++ b/src/array.c @@ -161,6 +161,16 @@ array_uint32_le (const unsigned char data[]) } +void +array_uint32_le_set (unsigned char data[], const unsigned int input) +{ + data[0] = input & 0xFF; + data[1] = (input >> 8) & 0xFF; + data[2] = (input >> 16) & 0xFF; + data[3] = (input >> 24) & 0xFF; +} + + unsigned int array_uint24_be (const unsigned char data[]) { @@ -168,6 +178,15 @@ array_uint24_be (const unsigned char data[]) } +void +array_uint24_be_set (unsigned char data[], const unsigned int input) +{ + data[0] = (input >> 16) & 0xFF; + data[1] = (input >> 8) & 0xFF; + data[2] = input & 0xFF; +} + + unsigned int array_uint24_le (const unsigned char data[]) { diff --git a/src/array.h b/src/array.h index b08ed2e..622e0d5 100644 --- a/src/array.h +++ b/src/array.h @@ -55,9 +55,15 @@ array_uint32_be (const unsigned char data[]); unsigned int array_uint32_le (const unsigned char data[]); +void +array_uint32_le_set (unsigned char data[], const unsigned int input); + unsigned int array_uint24_be (const unsigned char data[]); +void +array_uint24_be_set (unsigned char data[], const unsigned int input); + unsigned int array_uint24_le (const unsigned char data[]); diff --git a/src/hw_ostc3.c b/src/hw_ostc3.c index b78702c..a59fceb 100644 --- a/src/hw_ostc3.c +++ b/src/hw_ostc3.c @@ -2,6 +2,7 @@ * libdivecomputer * * Copyright (C) 2013 Jef Driesen + * Copyright (C) 2014 Anton Lundin * * This library is free software; you can redistribute it and/or * modify it under the terms of the GNU Lesser General Public @@ -21,15 +22,19 @@ #include // memcmp, memcpy #include // malloc, free +#include // FILE, fopen #include #include "context-private.h" #include "device-private.h" #include "serial.h" -#include "checksum.h" -#include "ringbuffer.h" #include "array.h" +#include "aes.h" + +#ifdef _MSC_VER +#define snprintf _snprintf +#endif #define ISINSTANCE(device) dc_device_isinstance((device), &hw_ostc3_device_vtable) @@ -43,11 +48,19 @@ #define SZ_VERSION (SZ_CUSTOMTEXT + 4) #define SZ_MEMORY 0x200000 #define SZ_CONFIG 4 +#define SZ_FIRMWARE 0x01E000 // 120KB +#define SZ_FIRMWARE_BLOCK 0x1000 // 4KB +#define FIRMWARE_AREA 0x3E0000 #define RB_LOGBOOK_SIZE 256 #define RB_LOGBOOK_COUNT 256 +#define S_BLOCK_READ 0x20 +#define S_BLOCK_WRITE 0x30 +#define S_ERASE 0x42 +#define S_READY 0x4C #define READY 0x4D +#define S_UPGRADE 0x50 #define HEADER 0x61 #define CLOCK 0x62 #define CUSTOMTEXT 0x63 @@ -60,12 +73,35 @@ #define INIT 0xBB #define EXIT 0xFF +typedef enum hw_ostc3_state_t { + OPEN, + DOWNLOAD, + SERVICE, + REBOOTING, +} hw_ostc3_state_t; + typedef struct hw_ostc3_device_t { dc_device_t base; serial_t *port; unsigned char fingerprint[5]; + hw_ostc3_state_t state; } 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); @@ -181,6 +217,7 @@ hw_ostc3_transfer (hw_ostc3_device_t *device, if (cmd != EXIT) { // Read the ready byte. unsigned char ready[1] = {0}; + unsigned char expected = (device->state == SERVICE ? S_READY : READY); n = serial_read (device->port, ready, sizeof (ready)); if (n != sizeof (ready)) { ERROR (abstract->context, "Failed to receive the ready byte."); @@ -188,7 +225,7 @@ hw_ostc3_transfer (hw_ostc3_device_t *device, } // Verify the ready byte. - if (ready[0] != READY) { + if (ready[0] != expected) { ERROR (abstract->context, "Unexpected ready byte."); return DC_STATUS_PROTOCOL; } @@ -247,14 +284,7 @@ hw_ostc3_device_open (dc_device_t **out, dc_context_t *context, const char *name serial_sleep (device->port, 300); serial_flush (device->port, SERIAL_QUEUE_BOTH); - // Send the init command. - dc_status_t status = hw_ostc3_transfer (device, NULL, INIT, NULL, 0, NULL, 0); - if (status != DC_STATUS_SUCCESS) { - ERROR (context, "Failed to send the command."); - serial_close (device->port); - free (device); - return status; - } + device->state = OPEN; *out = (dc_device_t *) device; @@ -262,18 +292,112 @@ hw_ostc3_device_open (dc_device_t **out, dc_context_t *context, const char *name } +static dc_status_t +hw_ostc3_device_init_download (hw_ostc3_device_t *device) +{ + dc_device_t *abstract = (dc_device_t *) device; + dc_context_t *context = (abstract ? abstract->context : NULL); + + // Send the init command. + dc_status_t status = hw_ostc3_transfer (device, NULL, INIT, NULL, 0, NULL, 0); + if (status != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to send the command."); + return status; + } + + device->state = DOWNLOAD; + + return DC_STATUS_SUCCESS; +} + + +static dc_status_t +hw_ostc3_device_init_service (hw_ostc3_device_t *device) +{ + dc_device_t *abstract = (dc_device_t *) device; + dc_context_t *context = (abstract ? abstract->context : NULL); + + unsigned char command[] = {0xAA, 0xAB, 0xCD, 0xEF}; + unsigned char output[5]; + int n = 0; + + // We cant use hw_ostc3_transfer here, due to the different echos + n = serial_write (device->port, command, sizeof (command)); + if (n != sizeof (command)) { + ERROR (context, "Failed to send the command."); + return EXITCODE (n); + } + + // Give the device some time to enter service mode + serial_sleep (device->port, 100); + + // Read the response + n = serial_read (device->port, output, sizeof (output)); + if (n != sizeof (output)) { + ERROR (context, "Failed to receive the echo."); + return EXITCODE (n); + } + + // Verify the response to service mode + if (output[0] != 0x4B || output[1] != 0xAB || + output[2] != 0xCD || output[3] != 0xEF || + output[4] != S_READY) { + ERROR (context, "Failed to verify echo."); + return DC_STATUS_IO; + } + + device->state = SERVICE; + + return DC_STATUS_SUCCESS; +} + + +static dc_status_t +hw_ostc3_device_init (hw_ostc3_device_t *device, hw_ostc3_state_t state) +{ + dc_status_t rc = DC_STATUS_SUCCESS; + + if (device->state == state) { + // No change. + rc = DC_STATUS_SUCCESS; + } else if (device->state == OPEN) { + // Change to download or service mode. + if (state == DOWNLOAD) { + rc = hw_ostc3_device_init_download(device); + } else if (state == SERVICE) { + rc = hw_ostc3_device_init_service(device); + } else { + rc = DC_STATUS_INVALIDARGS; + } + } else if (device->state == SERVICE && state == DOWNLOAD) { + // Switching between service and download mode is not possible. + // But in service mode, all download commands are supported too, + // so there is no need to change the state. + rc = DC_STATUS_SUCCESS; + } else { + // Not supported. + rc = DC_STATUS_INVALIDARGS; + } + + return rc; +} + + static dc_status_t hw_ostc3_device_close (dc_device_t *abstract) { hw_ostc3_device_t *device = (hw_ostc3_device_t*) abstract; + dc_status_t rc = DC_STATUS_SUCCESS; - // Send the exit command. - dc_status_t status = hw_ostc3_transfer (device, NULL, EXIT, NULL, 0, NULL, 0); - if (status != DC_STATUS_SUCCESS) { - ERROR (abstract->context, "Failed to send the command."); - serial_close (device->port); - free (device); - return status; + // Send the exit command + if (device->state == DOWNLOAD || device->state == SERVICE) { + rc = hw_ostc3_transfer (device, NULL, EXIT, NULL, 0, NULL, 0); + if (rc != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to send the command."); + serial_close (device->port); + free (device); + return rc; + } } // Close the device. @@ -317,8 +441,12 @@ hw_ostc3_device_version (dc_device_t *abstract, unsigned char data[], unsigned i if (size != SZ_VERSION) return DC_STATUS_INVALIDARGS; + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. - dc_status_t rc = hw_ostc3_transfer (device, NULL, IDENTITY, NULL, 0, data, size); + rc = hw_ostc3_transfer (device, NULL, IDENTITY, NULL, 0, data, size); if (rc != DC_STATUS_SUCCESS) return rc; @@ -336,9 +464,13 @@ hw_ostc3_device_foreach (dc_device_t *abstract, dc_dive_callback_t callback, voi progress.maximum = (RB_LOGBOOK_SIZE * RB_LOGBOOK_COUNT) + SZ_MEMORY; device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Download the version data. unsigned char id[SZ_VERSION] = {0}; - dc_status_t rc = hw_ostc3_device_version (abstract, id, sizeof (id)); + rc = hw_ostc3_device_version (abstract, id, sizeof (id)); if (rc != DC_STATUS_SUCCESS) { ERROR (abstract->context, "Failed to read the version."); return rc; @@ -503,11 +635,15 @@ hw_ostc3_device_clock (dc_device_t *abstract, const dc_datetime_t *datetime) return DC_STATUS_INVALIDARGS; } + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. unsigned char packet[6] = { datetime->hour, datetime->minute, datetime->second, datetime->month, datetime->day, datetime->year - 2000}; - dc_status_t rc = hw_ostc3_transfer (device, NULL, CLOCK, packet, sizeof (packet), NULL, 0); + rc = hw_ostc3_transfer (device, NULL, CLOCK, packet, sizeof (packet), NULL, 0); if (rc != DC_STATUS_SUCCESS) return rc; @@ -530,8 +666,12 @@ hw_ostc3_device_display (dc_device_t *abstract, const char *text) return DC_STATUS_INVALIDARGS; } + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. - dc_status_t rc = hw_ostc3_transfer (device, NULL, DISPLAY, packet, sizeof (packet), NULL, 0); + rc = hw_ostc3_transfer (device, NULL, DISPLAY, packet, sizeof (packet), NULL, 0); if (rc != DC_STATUS_SUCCESS) return rc; @@ -554,8 +694,12 @@ hw_ostc3_device_customtext (dc_device_t *abstract, const char *text) return DC_STATUS_INVALIDARGS; } + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. - dc_status_t rc = hw_ostc3_transfer (device, NULL, CUSTOMTEXT, packet, sizeof (packet), NULL, 0); + rc = hw_ostc3_transfer (device, NULL, CUSTOMTEXT, packet, sizeof (packet), NULL, 0); if (rc != DC_STATUS_SUCCESS) return rc; @@ -575,9 +719,13 @@ hw_ostc3_device_config_read (dc_device_t *abstract, unsigned int config, unsigne return DC_STATUS_INVALIDARGS; } + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. unsigned char command[1] = {config}; - dc_status_t rc = hw_ostc3_transfer (device, NULL, READ, command, sizeof (command), data, size); + rc = hw_ostc3_transfer (device, NULL, READ, command, sizeof (command), data, size); if (rc != DC_STATUS_SUCCESS) return rc; @@ -597,10 +745,14 @@ hw_ostc3_device_config_write (dc_device_t *abstract, unsigned int config, const return DC_STATUS_INVALIDARGS; } + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. unsigned char command[SZ_CONFIG + 1] = {config}; memcpy(command + 1, data, size); - dc_status_t rc = hw_ostc3_transfer (device, NULL, WRITE, command, size + 1, NULL, 0); + rc = hw_ostc3_transfer (device, NULL, WRITE, command, size + 1, NULL, 0); if (rc != DC_STATUS_SUCCESS) return rc; @@ -615,10 +767,348 @@ hw_ostc3_device_config_reset (dc_device_t *abstract) if (!ISINSTANCE (abstract)) return DC_STATUS_INVALIDARGS; + dc_status_t rc = hw_ostc3_device_init (device, DOWNLOAD); + if (rc != DC_STATUS_SUCCESS) + return rc; + // Send the command. - dc_status_t rc = hw_ostc3_transfer (device, NULL, RESET, NULL, 0, NULL, 0); + rc = hw_ostc3_transfer (device, NULL, RESET, NULL, 0, NULL, 0); if (rc != DC_STATUS_SUCCESS) return rc; return DC_STATUS_SUCCESS; } + +// This is a variant of fletcher16 with a 16 bit sum instead of an 8 bit sum, +// and modulo 2^16 instead of 2^16-1 +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, dc_context_t *context, unsigned int addr, unsigned char data[], unsigned int size) +{ + unsigned char ascii[39]; + unsigned char faddr_byte[3]; + unsigned int faddr = 0; + int n = 0; + + if (size > 16) { + ERROR (context, "Invalid arguments."); + return DC_STATUS_INVALIDARGS; + } + + // Read the start code. + while (1) { + n = fread (ascii, 1, 1, fp); + if (n != 1) { + ERROR (context, "Failed to read the start code."); + return DC_STATUS_IO; + } + + if (ascii[0] == ':') + break; + + // Ignore CR and LF characters. + if (ascii[0] != '\n' && ascii[0] != '\r') { + ERROR (context, "Unexpected character (0x%02x).", ascii[0]); + return DC_STATUS_DATAFORMAT; + } + } + + // Read the payload. + n = fread (ascii + 1, 1, 6 + size * 2, fp); + if (n != 6 + size * 2) { + ERROR (context, "Failed to read the data."); + return DC_STATUS_IO; + } + + // Convert the address to binary representation. + if (array_convert_hex2bin(ascii + 1, 6, faddr_byte, sizeof(faddr_byte)) != 0) { + ERROR (context, "Invalid hexadecimal character."); + return DC_STATUS_DATAFORMAT; + } + + // Get the address. + faddr = array_uint24_be (faddr_byte); + if (faddr != addr) { + ERROR (context, "Unexpected address (0x%06x, 0x%06x).", faddr, addr); + return DC_STATUS_DATAFORMAT; + } + + // Convert the payload to binary representation. + if (array_convert_hex2bin (ascii + 1 + 6, size * 2, data, size) != 0) { + ERROR (context, "Invalid hexadecimal character."); + 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 = NULL; + unsigned char iv[16] = {0}; + unsigned char tmpbuf[16] = {0}; + unsigned char encrypted[16] = {0}; + unsigned int bytes = 0, addr = 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, context, 0, iv, sizeof(iv)); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to parse header."); + fclose (fp); + 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, context, bytes, encrypted, sizeof(encrypted)); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to parse file data."); + fclose (fp); + return rc; + } + + // Decrypt AES-FCB data + for (unsigned 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, context, bytes, checksum, sizeof(checksum)); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to parse file tail."); + fclose (fp); + 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; +} + + +static dc_status_t +hw_ostc3_firmware_erase (hw_ostc3_device_t *device, unsigned int addr, unsigned int size) +{ + // Convert size to number of pages, rounded up. + unsigned char blocks = ((size + SZ_FIRMWARE_BLOCK - 1) / SZ_FIRMWARE_BLOCK); + + // Erase just the needed pages. + unsigned char buffer[4]; + array_uint24_be_set (buffer, addr); + buffer[3] = blocks; + + return hw_ostc3_transfer (device, NULL, S_ERASE, buffer, sizeof (buffer), NULL, 0); +} + +static dc_status_t +hw_ostc3_firmware_block_read (hw_ostc3_device_t *device, unsigned int addr, unsigned char block[], unsigned int block_size) +{ + unsigned char buffer[6]; + array_uint24_be_set (buffer, addr); + array_uint24_be_set (buffer + 3, block_size); + + return hw_ostc3_transfer (device, NULL, S_BLOCK_READ, buffer, sizeof (buffer), block, block_size); +} + +static dc_status_t +hw_ostc3_firmware_block_write (hw_ostc3_device_t *device, unsigned int addr, unsigned char block[], unsigned int block_size) +{ + unsigned char buffer[3 + SZ_FIRMWARE_BLOCK]; + + // We currenty only support writing max SZ_FIRMWARE_BLOCK sized blocks. + if (block_size > SZ_FIRMWARE_BLOCK) + return DC_STATUS_INVALIDARGS; + + array_uint24_be_set (buffer, addr); + memcpy (buffer + 3, block, block_size); + + return hw_ostc3_transfer (device, NULL, S_BLOCK_WRITE, buffer, 3 + block_size, NULL, 0); +} + +static dc_status_t +hw_ostc3_firmware_upgrade (dc_device_t *abstract, unsigned int checksum) +{ + dc_status_t rc = DC_STATUS_SUCCESS; + hw_ostc3_device_t *device = (hw_ostc3_device_t *) abstract; + dc_context_t *context = (abstract ? abstract->context : NULL); + unsigned char buffer[5]; + array_uint32_le_set (buffer, checksum); + + // Compute a one byte checksum, so the device can validate the firmware image. + buffer[4] = 0x55; + for (unsigned int i = 0; i < 4; i++) { + buffer[4] ^= buffer[i]; + buffer[4] = (buffer[4]<<1 | buffer[4]>>7); + } + + rc = hw_ostc3_transfer (device, NULL, S_UPGRADE, buffer, sizeof (buffer), NULL, 0); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to send flash firmware command"); + return rc; + } + + // Now the device resets, and if everything is well, it reprograms. + device->state = REBOOTING; + + return DC_STATUS_SUCCESS; +} + + +dc_status_t +hw_ostc3_device_fwupdate (dc_device_t *abstract, const char *filename) +{ + dc_status_t rc = DC_STATUS_SUCCESS; + hw_ostc3_device_t *device = (hw_ostc3_device_t *) abstract; + dc_context_t *context = (abstract ? abstract->context : NULL); + + // Enable progress notifications. + dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER; + + if (!ISINSTANCE (abstract)) + return DC_STATUS_INVALIDARGS; + + // load, erase, upload FZ, verify FZ, reprogram + progress.maximum = 3 + SZ_FIRMWARE * 2 / SZ_FIRMWARE_BLOCK; + device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + + // Allocate memory for the firmware data. + hw_ostc3_firmware_t *firmware = (hw_ostc3_firmware_t *) malloc (sizeof (hw_ostc3_firmware_t)); + if (firmware == NULL) { + ERROR (context, "Failed to allocate memory."); + return DC_STATUS_NOMEMORY; + } + + // Read the hex file. + rc = hw_ostc3_firmware_readfile (firmware, context, filename); + if (rc != DC_STATUS_SUCCESS) { + free (firmware); + return rc; + } + + // Make sure the device is in service mode + rc = hw_ostc3_device_init (device, SERVICE); + if (rc != DC_STATUS_SUCCESS) { + free (firmware); + return rc; + } + + // Device open and firmware loaded + progress.current++; + device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + + hw_ostc3_device_display (abstract, " Erasing FW..."); + + rc = hw_ostc3_firmware_erase (device, FIRMWARE_AREA, SZ_FIRMWARE); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to erase old firmware"); + free (firmware); + return rc; + } + + // Memory erased + progress.current++; + device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + + hw_ostc3_device_display (abstract, " Uploading..."); + + for (unsigned int len = 0; len < SZ_FIRMWARE; len += SZ_FIRMWARE_BLOCK) { + char status[SZ_DISPLAY + 1]; // Status message on the display + snprintf (status, sizeof(status), " Uploading %2d%%", (100 * len) / SZ_FIRMWARE); + hw_ostc3_device_display (abstract, status); + + rc = hw_ostc3_firmware_block_write (device, FIRMWARE_AREA + len, firmware->data + len, SZ_FIRMWARE_BLOCK); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to write block to device"); + free(firmware); + return rc; + } + // One block uploaded + progress.current++; + device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + } + + hw_ostc3_device_display (abstract, " Verifying..."); + + for (unsigned int len = 0; len < SZ_FIRMWARE; len += SZ_FIRMWARE_BLOCK) { + unsigned char block[SZ_FIRMWARE_BLOCK]; + char status[SZ_DISPLAY + 1]; // Status message on the display + snprintf (status, sizeof(status), " Verifying %2d%%", (100 * len) / SZ_FIRMWARE); + hw_ostc3_device_display (abstract, status); + + rc = hw_ostc3_firmware_block_read (device, FIRMWARE_AREA + len, block, sizeof (block)); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to read block."); + free (firmware); + return rc; + } + if (memcmp (firmware->data + len, block, sizeof (block)) != 0) { + ERROR (context, "Failed verify."); + hw_ostc3_device_display (abstract, " Verify FAILED"); + free (firmware); + return DC_STATUS_PROTOCOL; + } + // One block verified + progress.current++; + device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + } + + hw_ostc3_device_display (abstract, " Programming..."); + + rc = hw_ostc3_firmware_upgrade (abstract, firmware->checksum); + if (rc != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to start programing"); + free (firmware); + return rc; + } + + // Programing done! + progress.current++; + device_event_emit (abstract, DC_EVENT_PROGRESS, &progress); + + free (firmware); + + // Finished! + return DC_STATUS_SUCCESS; +} diff --git a/src/libdivecomputer.symbols b/src/libdivecomputer.symbols index 8dfd7be..8109f96 100644 --- a/src/libdivecomputer.symbols +++ b/src/libdivecomputer.symbols @@ -160,6 +160,7 @@ hw_ostc3_device_customtext hw_ostc3_device_config_read hw_ostc3_device_config_write hw_ostc3_device_config_reset +hw_ostc3_device_fwupdate zeagle_n2ition3_device_open atomics_cobalt_device_open atomics_cobalt_device_version