From afff8b450f80084f0a4eeee548430b1f3e67d204 Mon Sep 17 00:00:00 2001 From: Jef Driesen Date: Mon, 19 Feb 2018 22:58:53 +0100 Subject: [PATCH] Add BLE support for the Suunto Eon Steel devices The main difference with the USB HID communication is that the BLE data stream is encoded using HDLC framing with a 32 bit CRC checksum. Due to this encoding, the data packets can no longer be processed one by one (as is done for the USB HID packets). The entire HDLC encoded stream needs to be received before it can be processed. This requires some additional buffering. --- src/checksum.c | 45 +++++++++ src/checksum.h | 3 + src/descriptor.c | 4 +- src/suunto_eonsteel.c | 230 +++++++++++++++++++++++++++++++++++++++--- 4 files changed, 265 insertions(+), 17 deletions(-) diff --git a/src/checksum.c b/src/checksum.c index 8caf563..a8b332e 100644 --- a/src/checksum.c +++ b/src/checksum.c @@ -112,3 +112,48 @@ checksum_crc_ccitt_uint16 (const unsigned char data[], unsigned int size) return crc; } + +unsigned int +checksum_crc32 (const unsigned char data[], unsigned int size) +{ + static const unsigned int crc_table[] = { + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d, + }; + + unsigned int crc = 0xffffffff; + for (unsigned int i = 0; i < size; ++i) + crc = crc_table[(crc ^ data[i]) & 0xff] ^ (crc >> 8); + + return crc ^ 0xffffffff; +} diff --git a/src/checksum.h b/src/checksum.h index 4e54d45..a6ead09 100644 --- a/src/checksum.h +++ b/src/checksum.h @@ -41,6 +41,9 @@ checksum_xor_uint8 (const unsigned char data[], unsigned int size, unsigned char unsigned short checksum_crc_ccitt_uint16 (const unsigned char data[], unsigned int size); +unsigned int +checksum_crc32 (const unsigned char data[], unsigned int size); + #ifdef __cplusplus } #endif /* __cplusplus */ diff --git a/src/descriptor.c b/src/descriptor.c index 948a9f3..4ba943e 100644 --- a/src/descriptor.c +++ b/src/descriptor.c @@ -98,8 +98,8 @@ static const dc_descriptor_t g_descriptors[] = { {"Suunto", "Zoop Novo", DC_FAMILY_SUUNTO_D9, 0x1E, DC_TRANSPORT_SERIAL, NULL}, {"Suunto", "D4f", DC_FAMILY_SUUNTO_D9, 0x20, DC_TRANSPORT_SERIAL, NULL}, /* Suunto EON Steel */ - {"Suunto", "EON Steel", DC_FAMILY_SUUNTO_EONSTEEL, 0, DC_TRANSPORT_USBHID, dc_filter_suunto}, - {"Suunto", "EON Core", DC_FAMILY_SUUNTO_EONSTEEL, 1, DC_TRANSPORT_USBHID, dc_filter_suunto}, + {"Suunto", "EON Steel", DC_FAMILY_SUUNTO_EONSTEEL, 0, DC_TRANSPORT_USBHID | DC_TRANSPORT_BLE, dc_filter_suunto}, + {"Suunto", "EON Core", DC_FAMILY_SUUNTO_EONSTEEL, 1, DC_TRANSPORT_USBHID | DC_TRANSPORT_BLE, dc_filter_suunto}, /* Uwatec Aladin */ {"Uwatec", "Aladin Air Twin", DC_FAMILY_UWATEC_ALADIN, 0x1C, DC_TRANSPORT_SERIAL, NULL}, {"Uwatec", "Aladin Sport Plus", DC_FAMILY_UWATEC_ALADIN, 0x3E, DC_TRANSPORT_SERIAL, NULL}, diff --git a/src/suunto_eonsteel.c b/src/suunto_eonsteel.c index 6c430b4..336877b 100644 --- a/src/suunto_eonsteel.c +++ b/src/suunto_eonsteel.c @@ -28,6 +28,7 @@ #include "device-private.h" #include "array.h" #include "platform.h" +#include "checksum.h" #define EONSTEEL 0 #define EONCORE 1 @@ -76,6 +77,13 @@ struct directory_entry { #define PACKET_SIZE 64 #define HEADER_SIZE 12 +#define MAXDATA_SIZE 2048 +#define CRC_SIZE 4 + +// HDLC special characters +#define END 0x7E +#define ESC 0x7D +#define ESC_BIT 0x20 static dc_status_t suunto_eonsteel_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size); static dc_status_t suunto_eonsteel_device_foreach(dc_device_t *abstract, dc_dive_callback_t callback, void *userdata); @@ -133,6 +141,142 @@ static void put_le32(unsigned int val, unsigned char *p) p[3] = val >> 24; } +static dc_status_t +suunto_eonsteel_hdlc_write (suunto_eonsteel_device_t *device, const unsigned char data[], size_t size, size_t *actual) +{ + dc_status_t status = DC_STATUS_SUCCESS; + unsigned char buffer[20]; + size_t nbytes = 0; + + // Start of the packet. + buffer[nbytes++] = END; + + for (size_t i = 0; i < size; ++i) { + unsigned char c = data[i]; + + if (c == END || c == ESC) { + // Append the escape character. + buffer[nbytes++] = ESC; + + // Flush the buffer if necessary. + if (nbytes >= sizeof(buffer)) { + status = dc_iostream_write(device->iostream, buffer, nbytes, NULL); + if (status != DC_STATUS_SUCCESS) { + ERROR(device->base.context, "Failed to send the packet."); + return status; + } + + nbytes = 0; + } + + // Escape the character. + c ^= ESC_BIT; + } + + // Append the character. + buffer[nbytes++] = c; + + // Flush the buffer if necessary. + if (nbytes >= sizeof(buffer)) { + status = dc_iostream_write(device->iostream, buffer, nbytes, NULL); + if (status != DC_STATUS_SUCCESS) { + ERROR(device->base.context, "Failed to send the packet."); + return status; + } + + nbytes = 0; + } + } + + // End of the packet. + buffer[nbytes++] = END; + + // Flush the buffer. + status = dc_iostream_write(device->iostream, buffer, nbytes, NULL); + if (status != DC_STATUS_SUCCESS) { + ERROR(device->base.context, "Failed to send the packet."); + return status; + } + + if (actual) + *actual = size; + + return status; + +} + +static dc_status_t +suunto_eonsteel_hdlc_read (suunto_eonsteel_device_t *device, unsigned char data[], size_t size, size_t *actual) +{ + dc_status_t status = DC_STATUS_SUCCESS; + unsigned char buffer[20]; + unsigned int initialized = 0; + unsigned int escaped = 0; + size_t nbytes = 0; + + while (1) { + // Read a single data packet. + size_t transferred = 0; + status = dc_iostream_read(device->iostream, buffer, sizeof(buffer), &transferred); + if (status != DC_STATUS_SUCCESS) { + ERROR(device->base.context, "Failed to receive the packet."); + return status; + } + + for (size_t i = 0; i < transferred; ++i) { + unsigned char c = buffer[i]; + + if (c == END) { + if (escaped) { + ERROR (device->base.context, "HDLC frame escaped the special character %02x.", c); + return DC_STATUS_PROTOCOL; + } + + if (initialized) { + goto done; + } + + initialized = 1; + continue; + } + + if (!initialized) { + continue; + } + + if (c == ESC) { + if (escaped) { + ERROR (device->base.context, "HDLC frame escaped the special character %02x.", c); + return DC_STATUS_PROTOCOL; + } + escaped = 1; + continue; + } + + if (escaped) { + c ^= ESC_BIT; + escaped = 0; + } + + if (nbytes < size) + data[nbytes] = c; + nbytes++; + + } + } + +done: + if (nbytes > size) { + ERROR(device->base.context, "Insufficient buffer space available."); + return DC_STATUS_PROTOCOL; + } + + if (actual) + *actual = nbytes; + + return status; +} + /* * Get a single 64-byte packet from the dive computer. This handles packet * logging and any obvious packet-level errors, and returns the payload of @@ -144,7 +288,7 @@ static void put_le32(unsigned int val, unsigned char *p) * The maximum payload is 62 bytes. */ static dc_status_t -suunto_eonsteel_receive(suunto_eonsteel_device_t *device, unsigned char data[], unsigned int size, unsigned int *actual) +suunto_eonsteel_receive_usb(suunto_eonsteel_device_t *device, unsigned char data[], unsigned int size, unsigned int *actual) { dc_status_t rc = DC_STATUS_SUCCESS; unsigned char buf[PACKET_SIZE]; @@ -187,6 +331,48 @@ suunto_eonsteel_receive(suunto_eonsteel_device_t *device, unsigned char data[], return DC_STATUS_SUCCESS; } +static dc_status_t +suunto_eonsteel_receive_ble(suunto_eonsteel_device_t *device, unsigned char data[], unsigned int size, unsigned int *actual) +{ + dc_status_t rc = DC_STATUS_SUCCESS; + unsigned char buffer[HEADER_SIZE + MAXDATA_SIZE + CRC_SIZE]; + size_t transferred = 0; + + rc = suunto_eonsteel_hdlc_read(device, buffer, sizeof(buffer), &transferred); + if (rc != DC_STATUS_SUCCESS) { + ERROR(device->base.context, "Failed to receive the packet."); + return rc; + } + + if (transferred < CRC_SIZE) { + ERROR(device->base.context, "Invalid packet length (" DC_PRINTF_SIZE ").", transferred); + return DC_STATUS_PROTOCOL; + } + + unsigned int nbytes = transferred - CRC_SIZE; + + unsigned int crc = array_uint32_le(buffer + nbytes); + unsigned int ccrc = checksum_crc32(buffer, nbytes); + if (crc != ccrc) { + ERROR(device->base.context, "Invalid checksum (expected %08x, received %08x).", ccrc, crc); + return DC_STATUS_PROTOCOL; + } + + if (nbytes > size) { + ERROR(device->base.context, "Insufficient buffer space available."); + return DC_STATUS_PROTOCOL; + } + + memcpy(data, buffer, nbytes); + + HEXDUMP (device->base.context, DC_LOGLEVEL_DEBUG, "rcv", buffer, nbytes); + + if (actual) + *actual = nbytes; + + return DC_STATUS_SUCCESS; +} + static dc_status_t suunto_eonsteel_send(suunto_eonsteel_device_t *device, unsigned short cmd, @@ -194,11 +380,10 @@ suunto_eonsteel_send(suunto_eonsteel_device_t *device, unsigned int size) { dc_status_t rc = DC_STATUS_SUCCESS; - unsigned char buf[PACKET_SIZE]; - size_t transferred = 0; + unsigned char buf[PACKET_SIZE + CRC_SIZE]; // Two-byte packet header, followed by 12 bytes of extended header - if (size + 2 + HEADER_SIZE > sizeof(buf)) { + if (size + 2 + HEADER_SIZE + CRC_SIZE > sizeof(buf)) { ERROR(device->base.context, "Insufficient buffer space available."); return DC_STATUS_PROTOCOL; } @@ -225,7 +410,15 @@ suunto_eonsteel_send(suunto_eonsteel_device_t *device, memcpy(buf + 14, data, size); } - rc = dc_iostream_write(device->iostream, buf, sizeof(buf), &transferred); + // 4 byte LE checksum + unsigned int crc = checksum_crc32(buf + 2, size + HEADER_SIZE); + put_le32(crc, buf + 14 + size); + + if (dc_iostream_get_transport(device->iostream) == DC_TRANSPORT_BLE) { + rc = suunto_eonsteel_hdlc_write(device, buf + 2, size + HEADER_SIZE + CRC_SIZE, NULL); + } else { + rc = dc_iostream_write(device->iostream, buf, sizeof(buf) - CRC_SIZE, NULL); + } if (rc != DC_STATUS_SUCCESS) { ERROR(device->base.context, "Failed to send the command."); return rc; @@ -257,7 +450,7 @@ suunto_eonsteel_transfer(suunto_eonsteel_device_t *device, unsigned int *actual) { dc_status_t rc = DC_STATUS_SUCCESS; - unsigned char header[PACKET_SIZE]; + unsigned char header[HEADER_SIZE + MAXDATA_SIZE]; unsigned int len = 0; // Send the command. @@ -265,8 +458,13 @@ suunto_eonsteel_transfer(suunto_eonsteel_device_t *device, if (rc != DC_STATUS_SUCCESS) return rc; - // Receive the header and the first part of the data. - rc = suunto_eonsteel_receive(device, header, sizeof(header), &len); + if (dc_iostream_get_transport(device->iostream) == DC_TRANSPORT_BLE) { + // Receive the entire data packet. + rc = suunto_eonsteel_receive_ble(device, header, sizeof(header), &len); + } else { + // Receive the header and the first part of the data. + rc = suunto_eonsteel_receive_usb(device, header, sizeof(header), &len); + } if (rc != DC_STATUS_SUCCESS) return rc; @@ -319,15 +517,17 @@ suunto_eonsteel_transfer(suunto_eonsteel_device_t *device, memcpy(answer, header + HEADER_SIZE, nbytes); // Receive the remainder of the data. - while (nbytes < length) { - rc = suunto_eonsteel_receive(device, answer + nbytes, length - nbytes, &len); - if (rc != DC_STATUS_SUCCESS) - return rc; + if (dc_iostream_get_transport(device->iostream) != DC_TRANSPORT_BLE) { + while (nbytes < length) { + rc = suunto_eonsteel_receive_usb(device, answer + nbytes, length - nbytes, &len); + if (rc != DC_STATUS_SUCCESS) + return rc; - nbytes += len; + nbytes += len; - if (len < PACKET_SIZE - 2) - break; + if (len < PACKET_SIZE - 2) + break; + } } // Verify the total payload length.