Integrate the HDLC stream in the eonsteel backend
This commit is contained in:
Jef Driesen
parent
cee9a2e926
commit
b0e77fd05f
@ -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)
|
||||
{
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user