diff --git a/src/suunto_eonsteel.c b/src/suunto_eonsteel.c index 2f1335f..a18d27c 100644 --- a/src/suunto_eonsteel.c +++ b/src/suunto_eonsteel.c @@ -29,6 +29,7 @@ #include "array.h" #include "platform.h" #include "checksum.h" +#include "hdlc.h" #define EONSTEEL 0 #define EONCORE 1 @@ -80,14 +81,10 @@ struct directory_entry { #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); static dc_status_t suunto_eonsteel_device_timesync(dc_device_t *abstract, const dc_datetime_t *datetime); +static dc_status_t suunto_eonsteel_device_close (dc_device_t *abstract); static const dc_device_vtable_t suunto_eonsteel_device_vtable = { sizeof(suunto_eonsteel_device_t), @@ -98,7 +95,7 @@ static const dc_device_vtable_t suunto_eonsteel_device_vtable = { NULL, /* dump */ suunto_eonsteel_device_foreach, /* foreach */ suunto_eonsteel_device_timesync, /* timesync */ - NULL /* close */ + suunto_eonsteel_device_close /* close */ }; static const char dive_directory[] = "0:/dives"; @@ -127,142 +124,6 @@ static struct directory_entry *alloc_dirent(int type, int len, const char *name) return res; } -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 @@ -324,7 +185,7 @@ suunto_eonsteel_receive_ble(suunto_eonsteel_device_t *device, unsigned char data unsigned char buffer[HEADER_SIZE + MAXDATA_SIZE + CRC_SIZE]; size_t transferred = 0; - rc = suunto_eonsteel_hdlc_read(device, buffer, sizeof(buffer), &transferred); + rc = dc_iostream_read(device->iostream, buffer, sizeof(buffer), &transferred); if (rc != DC_STATUS_SUCCESS) { ERROR(device->base.context, "Failed to receive the packet."); return rc; @@ -401,7 +262,7 @@ suunto_eonsteel_send(suunto_eonsteel_device_t *device, array_uint32_le_set(buf + 14 + size, crc); if (dc_iostream_get_transport(device->iostream) == DC_TRANSPORT_BLE) { - rc = suunto_eonsteel_hdlc_write(device, buf + 2, size + HEADER_SIZE + CRC_SIZE, NULL); + rc = dc_iostream_write(device->iostream, buf + 2, size + HEADER_SIZE + CRC_SIZE, NULL); } else { rc = dc_iostream_write(device->iostream, buf, sizeof(buf) - CRC_SIZE, NULL); } @@ -757,6 +618,7 @@ suunto_eonsteel_device_open(dc_device_t **out, dc_context_t *context, dc_iostrea { dc_status_t status = DC_STATUS_SUCCESS; suunto_eonsteel_device_t *eon = NULL; + dc_transport_t transport = dc_iostream_get_transport (iostream); if (out == NULL) return DC_STATUS_INVALIDARGS; @@ -766,17 +628,26 @@ suunto_eonsteel_device_open(dc_device_t **out, dc_context_t *context, dc_iostrea return DC_STATUS_NOMEMORY; // Set up the magic handshake fields - eon->iostream = iostream; eon->model = model; eon->magic = INIT_MAGIC; eon->seq = INIT_SEQ; memset (eon->version, 0, sizeof (eon->version)); memset (eon->fingerprint, 0, sizeof (eon->fingerprint)); + if (transport == DC_TRANSPORT_BLE) { + status = dc_hdlc_open (&eon->iostream, context, iostream, 20, 20); + if (status != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to create the HDLC stream."); + goto error_free; + } + } else { + eon->iostream = iostream; + } + status = dc_iostream_set_timeout(eon->iostream, 5000); if (status != DC_STATUS_SUCCESS) { ERROR (context, "Failed to set the timeout."); - goto error_free; + goto error_free_iostream; } const unsigned char init[] = {0x02, 0x00, 0x2a, 0x00}; @@ -784,18 +655,34 @@ suunto_eonsteel_device_open(dc_device_t **out, dc_context_t *context, dc_iostrea init, sizeof(init), eon->version, sizeof(eon->version), NULL); if (status != DC_STATUS_SUCCESS) { ERROR(context, "unable to initialize device"); - goto error_free; + goto error_free_iostream; } *out = (dc_device_t *) eon; return DC_STATUS_SUCCESS; +error_free_iostream: + if (transport == DC_TRANSPORT_BLE) { + dc_iostream_close (eon->iostream); + } error_free: dc_device_deallocate ((dc_device_t *) eon); return status; } +static dc_status_t +suunto_eonsteel_device_close (dc_device_t *abstract) +{ + suunto_eonsteel_device_t *device = (suunto_eonsteel_device_t *) abstract; + + if (dc_iostream_get_transport (device->iostream) == DC_TRANSPORT_BLE) { + return dc_iostream_close (device->iostream); + } + + return DC_STATUS_SUCCESS; +} + static dc_status_t suunto_eonsteel_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size) {