static dc_status_t
oceanic_vtpro_send (oceanic_vtpro_device_t *device, const unsigned char command[], unsigned int csize)
{
dc_device_t *abstract = (dc_device_t *) device;
if (device_is_cancelled (abstract))
return DC_STATUS_CANCELLED;
// Send the command to the dive computer.
int n = serial_write (device->port, command, csize);
if (n != csize) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Receive the response (ACK/NAK) of the dive computer.
unsigned char response = NAK;
n = serial_read (device->port, &response, 1);
if (n != 1) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Verify the response of the dive computer.
if (response != ACK) {
ERROR (abstract->context, "Unexpected answer start byte(s).");
return DC_STATUS_PROTOCOL;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
hw_ostc_firmware_write_internal (hw_ostc_device_t *device, unsigned char *data, unsigned int size)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the packet.
int n = serial_write (device->port, data, size);
if (n != size) {
ERROR (abstract->context, "Failed to send the packet.");
return EXITCODE (n);
}
// Read the response.
unsigned char answer[1] = {0};
n = serial_read (device->port, answer, sizeof (answer));
if (n != sizeof (answer)) {
ERROR (abstract->context, "Failed to receive the response.");
return EXITCODE (n);
}
// Verify the response.
const unsigned char expected[] = {ACK};
if (memcmp (answer, expected, sizeof (expected)) != 0) {
ERROR (abstract->context, "Unexpected response.");
return DC_STATUS_PROTOCOL;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
hw_ostc_send (hw_ostc_device_t *device, unsigned char cmd, unsigned int echo)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command.
unsigned char command[1] = {cmd};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
if (echo) {
// Read the echo.
unsigned char answer[1] = {0};
n = serial_read (device->port, answer, sizeof (answer));
if (n != sizeof (answer)) {
ERROR (abstract->context, "Failed to receive the echo.");
return EXITCODE (n);
}
// Verify the echo.
if (memcmp (answer, command, sizeof (command)) != 0) {
ERROR (abstract->context, "Unexpected echo.");
return DC_STATUS_PROTOCOL;
}
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
oceanic_vtpro_init (oceanic_vtpro_device_t *device)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command to the dive computer.
unsigned char command[2] = {0xAA, 0x00};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Receive the answer of the dive computer.
unsigned char answer[13] = {0};
n = serial_read (device->port, answer, sizeof (answer));
if (n != sizeof (answer)) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Verify the answer.
const unsigned char response[13] = {
0x4D, 0x4F, 0x44, 0x2D, 0x2D, 0x4F, 0x4B,
0x5F, 0x56, 0x32, 0x2E, 0x30, 0x30};
if (memcmp (answer, response, sizeof (response)) != 0) {
ERROR (abstract->context, "Unexpected answer byte(s).");
return DC_STATUS_PROTOCOL;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
hw_ostc_firmware_setup_internal (hw_ostc_device_t *device)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command.
unsigned char command[1] = {0xC1};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Read the response.
unsigned char answer[2] = {0};
n = serial_read (device->port, answer, sizeof (answer));
if (n != sizeof (answer)) {
ERROR (abstract->context, "Failed to receive the response.");
return EXITCODE (n);
}
// Verify the response.
const unsigned char expected[2] = {PICTYPE, ACK};
if (memcmp (answer, expected, sizeof (expected)) != 0) {
ERROR (abstract->context, "Unexpected response.");
return DC_STATUS_PROTOCOL;
}
return DC_STATUS_SUCCESS;
}
static int
shearwater_common_slip_read (shearwater_common_device_t *device, unsigned char data[], unsigned int size)
{
unsigned int received = 0;
// Read bytes until a complete packet has been received. If the
// buffer runs out of space, bytes are dropped. The caller can
// detect this condition because the return value will be larger
// than the supplied buffer size.
while (1) {
unsigned char c = 0;
int n = 0;
// Get a single character to process.
n = serial_read (device->port, &c, 1);
if (n != 1) {
return EXITCODE(n);
}
switch (c) {
case END:
// If it's an END character then we're done.
// As a minor optimization, empty packets are ignored. This
// is to avoid bothering the upper layers with all the empty
// packets generated by the duplicate END characters which
// are sent to try to detect line noise.
if (received)
return received;
else
break;
case ESC:
// If it's an ESC character, get another character and then
// figure out what to store in the packet based on that.
n = serial_read (device->port, &c, 1);
if (n != 1) {
return EXITCODE(n);
}
// If it's not one of the two escaped characters, then we
// have a protocol violation. The best bet seems to be to
// leave the byte alone and just stuff it into the packet.
switch (c) {
case ESC_END:
c = END;
break;
case ESC_ESC:
c = ESC;
break;
}
// Fall-through!
default:
if (received < size)
data[received] = c;
received++;
}
}
return received;
}
static device_status_t
suunto_eon_device_dump (device_t *abstract, dc_buffer_t *buffer)
{
suunto_eon_device_t *device = (suunto_eon_device_t*) abstract;
if (! device_is_suunto_eon (abstract))
return DEVICE_STATUS_TYPE_MISMATCH;
// Erase the current contents of the buffer and
// pre-allocate the required amount of memory.
if (!dc_buffer_clear (buffer) || !dc_buffer_reserve (buffer, SUUNTO_EON_MEMORY_SIZE)) {
WARNING ("Insufficient buffer space available.");
return DEVICE_STATUS_MEMORY;
}
// Enable progress notifications.
device_progress_t progress = DEVICE_PROGRESS_INITIALIZER;
progress.maximum = SUUNTO_EON_MEMORY_SIZE + 1;
device_event_emit (abstract, DEVICE_EVENT_PROGRESS, &progress);
// Send the command.
unsigned char command[1] = {'P'};
int rc = serial_write (device->port, command, sizeof (command));
if (rc != sizeof (command)) {
WARNING ("Failed to send the command.");
return EXITCODE (rc);
}
// Receive the answer.
unsigned char answer[SUUNTO_EON_MEMORY_SIZE + 1] = {0};
rc = serial_read (device->port, answer, sizeof (answer));
if (rc != sizeof (answer)) {
WARNING ("Failed to receive the answer.");
return EXITCODE (rc);
}
// Update and emit a progress event.
progress.current += sizeof (answer);
device_event_emit (abstract, DEVICE_EVENT_PROGRESS, &progress);
// Verify the checksum of the package.
unsigned char crc = answer[sizeof (answer) - 1];
unsigned char ccrc = checksum_add_uint8 (answer, sizeof (answer) - 1, 0x00);
if (crc != ccrc) {
WARNING ("Unexpected answer CRC.");
return DEVICE_STATUS_PROTOCOL;
}
dc_buffer_append (buffer, answer, SUUNTO_EON_MEMORY_SIZE);
return DEVICE_STATUS_SUCCESS;
}
static int
shearwater_common_slip_write (shearwater_common_device_t *device, const unsigned char data[], unsigned int size)
{
int n = 0;
const unsigned char end[] = {END};
const unsigned char esc_end[] = {ESC, ESC_END};
const unsigned char esc_esc[] = {ESC, ESC_ESC};
#if 0
// Send an initial END character to flush out any data that may have
// accumulated in the receiver due to line noise.
n = serial_write (device->port, end, sizeof (end));
if (n != sizeof (end)) {
return EXITCODE(n);
}
#endif
for (unsigned int i = 0; i < size; ++i) {
const unsigned char *seq = NULL;
unsigned int len = 0;
switch (data[i]) {
case END:
// Escape the END character.
seq = esc_end;
len = sizeof (esc_end);
break;
case ESC:
// Escape the ESC character.
seq = esc_esc;
len = sizeof (esc_esc);
break;
default:
// Normal character.
seq = data + i;
len = 1;
break;
}
n = serial_write (device->port, seq, len);
if (n != len) {
return EXITCODE(n);
}
}
// Send the END character to indicate the end of the packet.
n = serial_write (device->port, end, sizeof (end));
if (n != sizeof (end)) {
return EXITCODE(n);
}
return size;
}
dc_status_t
atomics_cobalt_device_version (dc_device_t *abstract, unsigned char data[], unsigned int size)
{
atomics_cobalt_device_t *device = (atomics_cobalt_device_t *) abstract;
if (!ISINSTANCE (abstract))
return DC_STATUS_INVALIDARGS;
if (size < SZ_VERSION)
return DC_STATUS_INVALIDARGS;
#ifdef HAVE_LIBUSB
// Send the command to the dive computer.
uint8_t bRequest = 0x01;
int rc = libusb_control_transfer (device->handle,
LIBUSB_RECIPIENT_DEVICE | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_OUT,
bRequest, 0, 0, NULL, 0, TIMEOUT);
if (rc != LIBUSB_SUCCESS) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE(rc);
}
HEXDUMP (abstract->context, DC_LOGLEVEL_INFO, "Write", &bRequest, 1);
// Receive the answer from the dive computer.
int length = 0;
unsigned char packet[SZ_VERSION + 2] = {0};
rc = libusb_bulk_transfer (device->handle, 0x82,
packet, sizeof (packet), &length, TIMEOUT);
if (rc != LIBUSB_SUCCESS || length != sizeof (packet)) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE(rc);
}
HEXDUMP (abstract->context, DC_LOGLEVEL_INFO, "Read", packet, length);
// Verify the checksum of the packet.
unsigned short crc = array_uint16_le (packet + SZ_VERSION);
unsigned short ccrc = checksum_add_uint16 (packet, SZ_VERSION, 0x0);
if (crc != ccrc) {
ERROR (abstract->context, "Unexpected answer checksum.");
return DC_STATUS_PROTOCOL;
}
memcpy (data, packet, SZ_VERSION);
return DC_STATUS_SUCCESS;
#else
return DC_STATUS_UNSUPPORTED;
#endif
}
static dc_status_t
oceanic_vtpro_transfer (oceanic_vtpro_device_t *device, const unsigned char command[], unsigned int csize, unsigned char answer[], unsigned int asize)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command to the device. If the device responds with an
// ACK byte, the command was received successfully and the answer
// (if any) follows after the ACK byte. If the device responds with
// a NAK byte, we try to resend the command a number of times before
// returning an error.
unsigned int nretries = 0;
dc_status_t rc = DC_STATUS_SUCCESS;
while ((rc = oceanic_vtpro_send (device, command, csize)) != DC_STATUS_SUCCESS) {
if (rc != DC_STATUS_TIMEOUT && rc != DC_STATUS_PROTOCOL)
return rc;
// Abort if the maximum number of retries is reached.
if (nretries++ >= MAXRETRIES)
return rc;
}
// Receive the answer of the dive computer.
int n = serial_read (device->port, answer, asize);
if (n != asize) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
dc_status_t
hw_ostc_device_eeprom_read (dc_device_t *abstract, unsigned int bank, unsigned char data[], unsigned int size)
{
hw_ostc_device_t *device = (hw_ostc_device_t *) abstract;
if (!ISINSTANCE (abstract))
return DC_STATUS_INVALIDARGS;
if (bank > 2) {
ERROR (abstract->context, "Invalid eeprom bank specified.");
return DC_STATUS_INVALIDARGS;
}
if (size < SZ_EEPROM) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_INVALIDARGS;
}
// Send the command.
const unsigned char command[] = {'g', 'j', 'm'};
dc_status_t rc = hw_ostc_send (device, command[bank], 0);
if (rc != DC_STATUS_SUCCESS)
return rc;
// Read the answer.
int n = serial_read (device->port, data, SZ_EEPROM);
if (n != SZ_EEPROM) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
dc_status_t
hw_ostc_device_clock (dc_device_t *abstract, const dc_datetime_t *datetime)
{
hw_ostc_device_t *device = (hw_ostc_device_t *) abstract;
if (!ISINSTANCE (abstract))
return DC_STATUS_INVALIDARGS;
if (datetime == NULL) {
ERROR (abstract->context, "Invalid parameter specified.");
return DC_STATUS_INVALIDARGS;
}
// Send the command.
dc_status_t rc = hw_ostc_send (device, 'b', 1);
if (rc != DC_STATUS_SUCCESS)
return rc;
// Send the data packet.
unsigned char packet[6] = {
datetime->hour, datetime->minute, datetime->second,
datetime->month, datetime->day, datetime->year - 2000};
int n = serial_write (device->port, packet, sizeof (packet));
if (n != sizeof (packet)) {
ERROR (abstract->context, "Failed to send the data packet.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
dc_status_t
hw_ostc_device_md2hash (dc_device_t *abstract, unsigned char data[], unsigned int size)
{
hw_ostc_device_t *device = (hw_ostc_device_t *) abstract;
if (!ISINSTANCE (abstract))
return DC_STATUS_INVALIDARGS;
if (size < SZ_MD2HASH) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_INVALIDARGS;
}
// Send the command.
dc_status_t rc = hw_ostc_send (device, 'e', 0);
if (rc != DC_STATUS_SUCCESS)
return rc;
// Read the answer.
int n = serial_read (device->port, data, SZ_MD2HASH);
if (n != SZ_MD2HASH) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
diverite_nitekq_handshake (diverite_nitekq_device_t *device)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command.
unsigned char command[] = {HANDSHAKE};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Read the answer.
n = serial_read (device->port, device->version, sizeof (device->version));
if (n != sizeof (device->version)) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
diverite_nitekq_receive (diverite_nitekq_device_t *device, unsigned char data[], unsigned int size)
{
dc_device_t *abstract = (dc_device_t *) device;
// Read the answer.
int n = serial_read (device->port, data, size);
if (n != size) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Read the checksum.
unsigned char checksum[2] = {0};
n = serial_read (device->port, checksum, sizeof (checksum));
if (n != sizeof (checksum)) {
ERROR (abstract->context, "Failed to receive the checksum.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
device_status_t
suunto_eon_device_write_interval (device_t *abstract, unsigned char interval)
{
suunto_eon_device_t *device = (suunto_eon_device_t*) abstract;
if (! device_is_suunto_eon (abstract))
return DEVICE_STATUS_TYPE_MISMATCH;
// Send the command.
unsigned char command[2] = {'T', interval};
int rc = serial_write (device->port, command, sizeof (command));
if (rc != sizeof (command)) {
WARNING ("Failed to send the command.");
return EXITCODE (rc);
}
return DEVICE_STATUS_SUCCESS;
}
static dc_status_t
diverite_nitekq_send (diverite_nitekq_device_t *device, unsigned char cmd)
{
dc_device_t *abstract = (dc_device_t *) device;
if (device_is_cancelled (abstract))
return DC_STATUS_CANCELLED;
// Send the command.
unsigned char command[] = {cmd};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
reefnet_sensus_cancel (reefnet_sensus_device_t *device)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command to the device.
unsigned char command = 0x00;
int n = serial_write (device->port, &command, 1);
if (n != 1) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// The device leaves the waiting state.
device->waiting = 0;
return DC_STATUS_SUCCESS;
}
device_status_t
suunto_eon_device_write_name (device_t *abstract, unsigned char data[], unsigned int size)
{
suunto_eon_device_t *device = (suunto_eon_device_t*) abstract;
if (! device_is_suunto_eon (abstract))
return DEVICE_STATUS_TYPE_MISMATCH;
if (size > 20)
return DEVICE_STATUS_ERROR;
// Send the command.
unsigned char command[21] = {'N'};
memcpy (command + 1, data, size);
int rc = serial_write (device->port, command, sizeof (command));
if (rc != sizeof (command)) {
WARNING ("Failed to send the command.");
return EXITCODE (rc);
}
return DEVICE_STATUS_SUCCESS;
}
static dc_status_t
hw_ostc3_transfer (hw_ostc3_device_t *device,
dc_event_progress_t *progress,
unsigned char cmd,
const unsigned char input[],
unsigned int isize,
unsigned char output[],
unsigned int osize)
{
dc_device_t *abstract = (dc_device_t *) device;
if (device_is_cancelled (abstract))
return DC_STATUS_CANCELLED;
// Send the command.
unsigned char command[1] = {cmd};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Read the echo.
unsigned char echo[1] = {0};
n = serial_read (device->port, echo, sizeof (echo));
if (n != sizeof (echo)) {
ERROR (abstract->context, "Failed to receive the echo.");
return EXITCODE (n);
}
// Verify the echo.
if (memcmp (echo, command, sizeof (command)) != 0) {
ERROR (abstract->context, "Unexpected echo.");
return DC_STATUS_PROTOCOL;
}
if (input) {
// Send the input data packet.
n = serial_write (device->port, input, isize);
if (n != isize) {
ERROR (abstract->context, "Failed to send the data packet.");
return EXITCODE (n);
}
}
if (output) {
unsigned int nbytes = 0;
while (nbytes < osize) {
// Set the minimum packet size.
unsigned int len = 1024;
// Increase the packet size if more data is immediately available.
int available = serial_get_received (device->port);
if (available > len)
len = available;
// Limit the packet size to the total size.
if (nbytes + len > osize)
len = osize - nbytes;
// Read the packet.
n = serial_read (device->port, output + nbytes, len);
if (n != len) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Update and emit a progress event.
if (progress) {
progress->current += len;
device_event_emit ((dc_device_t *) device, DC_EVENT_PROGRESS, progress);
}
nbytes += len;
}
}
if (cmd != EXIT) {
// Read the ready byte.
unsigned char ready[1] = {0};
n = serial_read (device->port, ready, sizeof (ready));
if (n != sizeof (ready)) {
ERROR (abstract->context, "Failed to receive the ready byte.");
return EXITCODE (n);
}
// Verify the ready byte.
if (ready[0] != READY) {
ERROR (abstract->context, "Unexpected ready byte.");
return DC_STATUS_PROTOCOL;
}
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
reefnet_sensus_device_dump (dc_device_t *abstract, dc_buffer_t *buffer)
{
reefnet_sensus_device_t *device = (reefnet_sensus_device_t*) abstract;
// Erase the current contents of the buffer and
// pre-allocate the required amount of memory.
if (!dc_buffer_clear (buffer) || !dc_buffer_reserve (buffer, SZ_MEMORY)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Enable progress notifications.
dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER;
progress.maximum = 4 + SZ_MEMORY + 2 + 3;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
// Wake-up the device.
dc_status_t rc = reefnet_sensus_handshake (device);
if (rc != DC_STATUS_SUCCESS)
return rc;
// Send the command to the device.
unsigned char command = 0x40;
int n = serial_write (device->port, &command, 1);
if (n != 1) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// The device leaves the waiting state.
device->waiting = 0;
// Receive the answer from the device.
unsigned int nbytes = 0;
unsigned char answer[4 + SZ_MEMORY + 2 + 3] = {0};
while (nbytes < sizeof (answer)) {
unsigned int len = sizeof (answer) - nbytes;
if (len > 128)
len = 128;
n = serial_read (device->port, answer + nbytes, len);
if (n != len) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Update and emit a progress event.
progress.current += len;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
nbytes += len;
}
// Verify the headers of the package.
if (memcmp (answer, "DATA", 4) != 0 ||
memcmp (answer + sizeof (answer) - 3, "END", 3) != 0) {
ERROR (abstract->context, "Unexpected answer start or end byte(s).");
return DC_STATUS_PROTOCOL;
}
// Verify the checksum of the package.
unsigned short crc = array_uint16_le (answer + 4 + SZ_MEMORY);
unsigned short ccrc = checksum_add_uint16 (answer + 4, SZ_MEMORY, 0x00);
if (crc != ccrc) {
ERROR (abstract->context, "Unexpected answer checksum.");
return DC_STATUS_PROTOCOL;
}
dc_buffer_append (buffer, answer + 4, SZ_MEMORY);
return DC_STATUS_SUCCESS;
}
static dc_status_t
reefnet_sensus_handshake (reefnet_sensus_device_t *device)
{
dc_device_t *abstract = (dc_device_t *) device;
// Send the command to the device.
unsigned char command = 0x0A;
int n = serial_write (device->port, &command, 1);
if (n != 1) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Receive the answer from the device.
unsigned char handshake[SZ_HANDSHAKE + 2] = {0};
n = serial_read (device->port, handshake, sizeof (handshake));
if (n != sizeof (handshake)) {
ERROR (abstract->context, "Failed to receive the handshake.");
return EXITCODE (n);
}
// Verify the header of the packet.
if (handshake[0] != 'O' || handshake[1] != 'K') {
ERROR (abstract->context, "Unexpected answer header.");
return DC_STATUS_PROTOCOL;
}
// The device is now waiting for a data request.
device->waiting = 1;
// Store the clock calibration values.
device->systime = dc_datetime_now ();
device->devtime = array_uint32_le (handshake + 8);
// Store the handshake packet.
memcpy (device->handshake, handshake + 2, SZ_HANDSHAKE);
// Emit a clock event.
dc_event_clock_t clock;
clock.systime = device->systime;
clock.devtime = device->devtime;
device_event_emit (&device->base, DC_EVENT_CLOCK, &clock);
// Emit a device info event.
dc_event_devinfo_t devinfo;
devinfo.model = handshake[2] - '0';
devinfo.firmware = handshake[3] - '0';
devinfo.serial = array_uint16_le (handshake + 6);
device_event_emit (&device->base, DC_EVENT_DEVINFO, &devinfo);
// Emit a vendor event.
dc_event_vendor_t vendor;
vendor.data = device->handshake;
vendor.size = sizeof (device->handshake);
device_event_emit (abstract, DC_EVENT_VENDOR, &vendor);
// Wait at least 10 ms to ensures the data line is
// clear before transmission from the host begins.
serial_sleep (device->port, 10);
return DC_STATUS_SUCCESS;
}
static device_status_t
suunto_d9_device_packet (device_t *abstract, const unsigned char command[], unsigned int csize, unsigned char answer[], unsigned int asize, unsigned int size)
{
suunto_d9_device_t *device = (suunto_d9_device_t *) abstract;
if (device_is_cancelled (abstract))
return DEVICE_STATUS_CANCELLED;
// Clear RTS to send the command.
serial_set_rts (device->port, 0);
// Send the command to the dive computer.
int n = serial_write (device->port, command, csize);
if (n != csize) {
WARNING ("Failed to send the command.");
return EXITCODE (n);
}
// Wait until all data has been transmitted.
serial_drain (device->port);
// Receive the echo.
unsigned char echo[128] = {0};
assert (sizeof (echo) >= csize);
n = serial_read (device->port, echo, csize);
if (n != csize) {
WARNING ("Failed to receive the echo.");
return EXITCODE (n);
}
// Verify the echo.
if (memcmp (command, echo, csize) != 0) {
WARNING ("Unexpected echo.");
return DEVICE_STATUS_PROTOCOL;
}
// Set RTS to receive the reply.
serial_set_rts (device->port, 1);
// Receive the answer of the dive computer.
n = serial_read (device->port, answer, asize);
if (n != asize) {
WARNING ("Failed to receive the answer.");
return EXITCODE (n);
}
// Verify the header of the package.
if (answer[0] != command[0]) {
WARNING ("Unexpected answer header.");
return DEVICE_STATUS_PROTOCOL;
}
// Verify the size of the package.
if (array_uint16_be (answer + 1) + 4 != asize) {
WARNING ("Unexpected answer size.");
return DEVICE_STATUS_PROTOCOL;
}
// Verify the parameters of the package.
if (memcmp (command + 3, answer + 3, asize - size - 4) != 0) {
WARNING ("Unexpected answer parameters.");
return DEVICE_STATUS_PROTOCOL;
}
// Verify the checksum of the package.
unsigned char crc = answer[asize - 1];
unsigned char ccrc = checksum_xor_uint8 (answer, asize - 1, 0x00);
if (crc != ccrc) {
WARNING ("Unexpected answer CRC.");
return DEVICE_STATUS_PROTOCOL;
}
return DEVICE_STATUS_SUCCESS;
}
dc_status_t
hw_ostc_device_screenshot (dc_device_t *abstract, dc_buffer_t *buffer, hw_ostc_format_t format)
{
hw_ostc_device_t *device = (hw_ostc_device_t *) abstract;
if (!ISINSTANCE (abstract))
return DC_STATUS_INVALIDARGS;
// Erase the current contents of the buffer.
if (!dc_buffer_clear (buffer)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Bytes per pixel (RGB formats only).
unsigned int bpp = 0;
if (format == HW_OSTC_FORMAT_RAW) {
// The RAW format has a variable size, depending on the actual image
// content. Usually the total size is around 4K, which is used as an
// initial guess and expanded when necessary.
if (!dc_buffer_reserve (buffer, 4096)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
} else {
// The RGB formats have a fixed size, depending only on the dimensions
// and the number of bytes per pixel. The required amount of memory is
// allocated immediately.
bpp = (format == HW_OSTC_FORMAT_RGB16) ? 2 : 3;
if (!dc_buffer_resize (buffer, WIDTH * HEIGHT * bpp)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
}
// Enable progress notifications.
dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER;
progress.maximum = WIDTH * HEIGHT;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
// Send the command.
dc_status_t rc = hw_ostc_send (device, 'l', 1);
if (rc != DC_STATUS_SUCCESS)
return rc;
// Cache the pointer to the image data (RGB formats only).
unsigned char *image = dc_buffer_get_data (buffer);
// The OSTC sends the image data in a column by column layout, which is
// converted on the fly to a more convenient row by row layout as used
// in the majority of image formats. This conversions requires knowledge
// of the pixel coordinates.
unsigned int x = 0, y = 0;
unsigned int npixels = 0;
while (npixels < WIDTH * HEIGHT) {
unsigned char raw[3] = {0};
int n = serial_read (device->port, raw, 1);
if (n != 1) {
ERROR (abstract->context, "Failed to receive the packet.");
return EXITCODE (n);
}
unsigned int nbytes = n;
unsigned int count = raw[0];
if ((count & 0x80) == 0x00) {
// Black pixel.
raw[1] = raw[2] = BLACK;
count &= 0x7F;
} else if ((count & 0xC0) == 0xC0) {
// White pixel.
raw[1] = raw[2] = WHITE;
count &= 0x3F;
} else {
// Color pixel.
n = serial_read (device->port, raw + 1, 2);
if (n != 2) {
ERROR (abstract->context, "Failed to receive the packet.");
return EXITCODE (n);
}
nbytes += n;
count &= 0x3F;
}
count++;
// Check for buffer overflows.
if (npixels + count > WIDTH * HEIGHT) {
ERROR (abstract->context, "Unexpected number of pixels received.");
return DC_STATUS_DATAFORMAT;
}
if (format == HW_OSTC_FORMAT_RAW) {
// Append the raw data to the output buffer.
dc_buffer_append (buffer, raw, nbytes);
} else {
// Store the decompressed data in the output buffer.
for (unsigned int i = 0; i < count; ++i) {
// Calculate the offset to the current pixel (row layout)
unsigned int offset = (y * WIDTH + x) * bpp;
if (format == HW_OSTC_FORMAT_RGB16) {
image[offset + 0] = raw[1];
image[offset + 1] = raw[2];
} else {
unsigned int value = (raw[1] << 8) + raw[2];
unsigned char r = (value & 0xF800) >> 11;
unsigned char g = (value & 0x07E0) >> 5;
unsigned char b = (value & 0x001F);
image[offset + 0] = 255 * r / 31;
image[offset + 1] = 255 * g / 63;
image[offset + 2] = 255 * b / 31;
}
// Move to the next pixel coordinate (column layout).
y++;
if (y == HEIGHT) {
y = 0;
x++;
}
}
}
// Update and emit a progress event.
progress.current += count;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
npixels += count;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
cressi_leonardo_device_dump (dc_device_t *abstract, dc_buffer_t *buffer)
{
cressi_leonardo_device_t *device = (cressi_leonardo_device_t *) abstract;
// Erase the current contents of the buffer and
// pre-allocate the required amount of memory.
if (!dc_buffer_clear (buffer) || !dc_buffer_resize (buffer, SZ_MEMORY)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Enable progress notifications.
dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER;
progress.maximum = SZ_MEMORY;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
// Send the command header to the dive computer.
const unsigned char command[] = {0x7B, 0x31, 0x32, 0x33, 0x44, 0x42, 0x41, 0x7d};
int n = serial_write (device->port, command, sizeof (command));
if (n != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Receive the header packet.
unsigned char header[7] = {0};
n = serial_read (device->port, header, sizeof (header));
if (n != sizeof (header)) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Verify the header packet.
const unsigned char expected[] = {0x7B, 0x21, 0x44, 0x35, 0x42, 0x33, 0x7d};
if (memcmp (header, expected, sizeof (expected)) != 0) {
ERROR (abstract->context, "Unexpected answer byte.");
return DC_STATUS_PROTOCOL;
}
unsigned char *data = dc_buffer_get_data (buffer);
unsigned int nbytes = 0;
while (nbytes < SZ_MEMORY) {
// Set the minimum packet size.
unsigned int len = 1024;
// Increase the packet size if more data is immediately available.
int available = serial_get_received (device->port);
if (available > len)
len = available;
// Limit the packet size to the total size.
if (nbytes + len > SZ_MEMORY)
len = SZ_MEMORY - nbytes;
// Read the packet.
n = serial_read (device->port, data + nbytes, len);
if (n != len) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Update and emit a progress event.
progress.current += len;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
nbytes += len;
}
// Receive the trailer packet.
unsigned char trailer[4] = {0};
n = serial_read (device->port, trailer, sizeof (trailer));
if (n != sizeof (trailer)) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Convert to a binary checksum.
unsigned char checksum[2] = {0};
array_convert_hex2bin (trailer, sizeof (trailer), checksum, sizeof (checksum));
// Verify the checksum.
unsigned int csum1 = array_uint16_be (checksum);
unsigned int csum2 = checksum_crc_ccitt_uint16 (data, SZ_MEMORY);
if (csum1 != csum2) {
ERROR (abstract->context, "Unexpected answer bytes.");
return DC_STATUS_PROTOCOL;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
atomics_cobalt_read_dive (dc_device_t *abstract, dc_buffer_t *buffer, int init, dc_event_progress_t *progress)
{
#ifdef HAVE_LIBUSB
atomics_cobalt_device_t *device = (atomics_cobalt_device_t *) abstract;
if (device_is_cancelled (abstract))
return DC_STATUS_CANCELLED;
// Erase the current contents of the buffer.
if (!dc_buffer_clear (buffer)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Send the command to the dive computer.
uint8_t bRequest = 0;
if (device->simulation)
bRequest = init ? 0x02 : 0x03;
else
bRequest = init ? 0x09 : 0x0A;
int rc = libusb_control_transfer (device->handle,
LIBUSB_RECIPIENT_DEVICE | LIBUSB_REQUEST_TYPE_VENDOR | LIBUSB_ENDPOINT_OUT,
bRequest, 0, 0, NULL, 0, TIMEOUT);
if (rc != LIBUSB_SUCCESS) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE(rc);
}
HEXDUMP (abstract->context, DC_LOGLEVEL_INFO, "Write", &bRequest, 1);
unsigned int nbytes = 0;
while (1) {
// Receive the answer from the dive computer.
int length = 0;
unsigned char packet[8 * 1024] = {0};
rc = libusb_bulk_transfer (device->handle, 0x82,
packet, sizeof (packet), &length, TIMEOUT);
if (rc != LIBUSB_SUCCESS) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE(rc);
}
HEXDUMP (abstract->context, DC_LOGLEVEL_INFO, "Read", packet, length);
// Update and emit a progress event.
if (progress) {
progress->current += length;
device_event_emit (abstract, DC_EVENT_PROGRESS, progress);
}
// Append the packet to the output buffer.
dc_buffer_append (buffer, packet, length);
nbytes += length;
// If we received fewer bytes than requested, the transfer is finished.
if (length < sizeof (packet))
break;
}
// Check for a buffer error.
if (dc_buffer_get_size (buffer) != nbytes) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Check for the minimum length.
if (nbytes < 2) {
ERROR (abstract->context, "Data packet is too short.");
return DC_STATUS_PROTOCOL;
}
// When only two 0xFF bytes are received, there are no more dives.
unsigned char *data = dc_buffer_get_data (buffer);
if (nbytes == 2 && data[0] == 0xFF && data[1] == 0xFF) {
dc_buffer_clear (buffer);
return DC_STATUS_SUCCESS;
}
// Verify the checksum of the packet.
unsigned short crc = array_uint16_le (data + nbytes - 2);
unsigned short ccrc = checksum_add_uint16 (data, nbytes - 2, 0x0);
if (crc != ccrc) {
ERROR (abstract->context, "Unexpected answer checksum.");
return DC_STATUS_PROTOCOL;
}
// Remove the checksum bytes.
dc_buffer_slice (buffer, 0, nbytes - 2);
return DC_STATUS_SUCCESS;
#else
return DC_STATUS_UNSUPPORTED;
#endif
}
static dc_status_t
uwatec_meridian_device_dump (dc_device_t *abstract, dc_buffer_t *buffer)
{
uwatec_meridian_device_t *device = (uwatec_meridian_device_t*) abstract;
dc_status_t rc = DC_STATUS_SUCCESS;
// Erase the current contents of the buffer.
if (!dc_buffer_clear (buffer)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Enable progress notifications.
dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER;
device_event_emit (&device->base, DC_EVENT_PROGRESS, &progress);
// Command template.
unsigned char command[9] = {0x00,
(device->timestamp ) & 0xFF,
(device->timestamp >> 8 ) & 0xFF,
(device->timestamp >> 16) & 0xFF,
(device->timestamp >> 24) & 0xFF,
0x10,
0x27,
0,
0};
// Read the model number.
command[0] = 0x10;
unsigned char model[1] = {0};
rc = uwatec_meridian_transfer (device, command, 1, model, sizeof (model));
if (rc != DC_STATUS_SUCCESS)
return rc;
// Read the serial number.
command[0] = 0x14;
unsigned char serial[4] = {0};
rc = uwatec_meridian_transfer (device, command, 1, serial, sizeof (serial));
if (rc != DC_STATUS_SUCCESS)
return rc;
// Read the device clock.
command[0] = 0x1A;
unsigned char devtime[4] = {0};
rc = uwatec_meridian_transfer (device, command, 1, devtime, sizeof (devtime));
if (rc != DC_STATUS_SUCCESS)
return rc;
// Store the clock calibration values.
device->systime = dc_datetime_now ();
device->devtime = array_uint32_le (devtime);
// Update and emit a progress event.
progress.current += 9;
device_event_emit (&device->base, DC_EVENT_PROGRESS, &progress);
// Emit a clock event.
dc_event_clock_t clock;
clock.systime = device->systime;
clock.devtime = device->devtime;
device_event_emit (&device->base, DC_EVENT_CLOCK, &clock);
// Emit a device info event.
dc_event_devinfo_t devinfo;
devinfo.model = model[0];
devinfo.firmware = 0;
devinfo.serial = array_uint32_le (serial);
device_event_emit (&device->base, DC_EVENT_DEVINFO, &devinfo);
// Data Length.
command[0] = 0xC6;
unsigned char answer[4] = {0};
rc = uwatec_meridian_transfer (device, command, sizeof (command), answer, sizeof (answer));
if (rc != DC_STATUS_SUCCESS)
return rc;
unsigned int length = array_uint32_le (answer);
// Update and emit a progress event.
progress.maximum = 4 + 9 + (length ? length + 4 : 0);
progress.current += 4;
device_event_emit (&device->base, DC_EVENT_PROGRESS, &progress);
if (length == 0)
return DC_STATUS_SUCCESS;
// Allocate the required amount of memory.
if (!dc_buffer_resize (buffer, length)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
unsigned char *data = dc_buffer_get_data (buffer);
// Data.
command[0] = 0xC4;
rc = uwatec_meridian_transfer (device, command, sizeof (command), answer, sizeof (answer));
if (rc != DC_STATUS_SUCCESS)
return rc;
unsigned int total = array_uint32_le (answer);
// Update and emit a progress event.
progress.current += 4;
device_event_emit (&device->base, DC_EVENT_PROGRESS, &progress);
if (total != length + 4) {
ERROR (abstract->context, "Received an unexpected size.");
return DC_STATUS_PROTOCOL;
}
unsigned int nbytes = 0;
while (nbytes < length) {
// Read the header.
unsigned char header[5];
int n = serial_read (device->port, header, sizeof (header));
if (n != sizeof (header)) {
ERROR (abstract->context, "Failed to receive the header.");
return EXITCODE (n);
}
// Get the packet size.
unsigned int packetsize = array_uint32_le (header);
if (packetsize < 1 || nbytes + packetsize - 1 > length) {
WARNING (abstract->context, "Unexpected header.");
return DC_STATUS_PROTOCOL;
}
// Read the packet data.
n = serial_read (device->port, data + nbytes, packetsize - 1);
if (n != packetsize - 1) {
ERROR (abstract->context, "Failed to receive the packet.");
return EXITCODE (n);
}
// Read the checksum.
unsigned char csum = 0x00;
n = serial_read (device->port, &csum, sizeof (csum));
if (n != sizeof (csum)) {
ERROR (abstract->context, "Failed to receive the checksum.");
return EXITCODE (n);
}
// Verify the checksum.
unsigned char ccsum = 0x00;
ccsum = checksum_xor_uint8 (header, sizeof (header), ccsum);
ccsum = checksum_xor_uint8 (data + nbytes, packetsize - 1, ccsum);
if (csum != ccsum) {
ERROR (abstract->context, "Unexpected answer checksum.");
return DC_STATUS_PROTOCOL;
}
// Update and emit a progress event.
progress.current += packetsize - 1;
device_event_emit (&device->base, DC_EVENT_PROGRESS, &progress);
nbytes += packetsize - 1;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
hw_ostc_device_dump (dc_device_t *abstract, dc_buffer_t *buffer)
{
hw_ostc_device_t *device = (hw_ostc_device_t*) abstract;
// Erase the current contents of the buffer.
if (!dc_buffer_clear (buffer)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
// Enable progress notifications.
dc_event_progress_t progress = EVENT_PROGRESS_INITIALIZER;
progress.maximum = SZ_HEADER + SZ_FW_NEW;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
// Send the command.
unsigned char command[1] = {'a'};
int rc = serial_write (device->port, command, sizeof (command));
if (rc != sizeof (command)) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (rc);
}
// Read the header.
unsigned char header[SZ_HEADER] = {0};
int n = serial_read (device->port, header, sizeof (header));
if (n != sizeof (header)) {
ERROR (abstract->context, "Failed to receive the header.");
return EXITCODE (n);
}
// Verify the header.
unsigned char preamble[] = {0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0x55};
if (memcmp (header, preamble, sizeof (preamble)) != 0) {
ERROR (abstract->context, "Unexpected answer header.");
return DC_STATUS_DATAFORMAT;
}
// Get the firmware version.
unsigned int firmware = array_uint16_be (header + 264);
// Get the amount of profile data.
unsigned int size = sizeof (header);
if (firmware > FW_190)
size += SZ_FW_NEW;
else
size += SZ_FW_190;
// Update and emit a progress event.
progress.current = sizeof (header);
progress.maximum = size;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
// Allocate the required amount of memory.
if (!dc_buffer_resize (buffer, size)) {
ERROR (abstract->context, "Insufficient buffer space available.");
return DC_STATUS_NOMEMORY;
}
unsigned char *data = dc_buffer_get_data (buffer);
// Copy the header to the output buffer.
memcpy (data, header, sizeof (header));
unsigned int nbytes = sizeof (header);
while (nbytes < size) {
// Set the minimum packet size.
unsigned int len = 1024;
// Increase the packet size if more data is immediately available.
int available = serial_get_received (device->port);
if (available > len)
len = available;
// Limit the packet size to the total size.
if (nbytes + len > size)
len = size - nbytes;
// Read the packet.
int n = serial_read (device->port, data + nbytes, len);
if (n != len) {
ERROR (abstract->context, "Failed to receive the answer.");
return EXITCODE (n);
}
// Update and emit a progress event.
progress.current += len;
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
nbytes += len;
}
return DC_STATUS_SUCCESS;
}
static dc_status_t
uwatec_meridian_transfer (uwatec_meridian_device_t *device, const unsigned char command[], unsigned int csize, unsigned char answer[], unsigned int asize)
{
dc_device_t *abstract = (dc_device_t *) device;
assert (csize > 0 && csize <= 255);
// Build the packet.
unsigned char packet[255 + 12] = {
0xFF, 0xFF, 0xFF,
0xA6, 0x59, 0xBD, 0xC2,
0x00, /* length */
0x00, 0x00, 0x00,
0x00}; /* data and checksum */
memcpy (packet + 11, command, csize);
packet[7] = csize;
packet[11 + csize] = checksum_xor_uint8 (packet + 7, csize + 4, 0x00);
// Send the packet.
int n = serial_write (device->port, packet, csize + 12);
if (n != csize + 12) {
ERROR (abstract->context, "Failed to send the command.");
return EXITCODE (n);
}
// Read the echo.
unsigned char echo[sizeof(packet)];
n = serial_read (device->port, echo, csize + 12);
if (n != csize + 12) {
ERROR (abstract->context, "Failed to receive the echo.");
return EXITCODE (n);
}
// Verify the echo.
if (memcmp (echo, packet, csize + 12) != 0) {
WARNING (abstract->context, "Unexpected echo.");
return DC_STATUS_PROTOCOL;
}
// Read the header.
unsigned char header[6];
n = serial_read (device->port, header, sizeof (header));
if (n != sizeof (header)) {
ERROR (abstract->context, "Failed to receive the header.");
return EXITCODE (n);
}
// Verify the header.
if (header[0] != ACK || array_uint32_le (header + 1) != asize + 1 || header[5] != packet[11]) {
WARNING (abstract->context, "Unexpected header.");
return DC_STATUS_PROTOCOL;
}
// Read the packet.
n = serial_read (device->port, answer, asize);
if (n != asize) {
ERROR (abstract->context, "Failed to receive the packet.");
return EXITCODE (n);
}
// Read the checksum.
unsigned char csum = 0x00;
n = serial_read (device->port, &csum, sizeof (csum));
if (n != sizeof (csum)) {
ERROR (abstract->context, "Failed to receive the checksum.");
return EXITCODE (n);
}
// Verify the checksum.
unsigned char ccsum = 0x00;
ccsum = checksum_xor_uint8 (header + 1, sizeof (header) - 1, ccsum);
ccsum = checksum_xor_uint8 (answer, asize, ccsum);
if (csum != ccsum) {
ERROR (abstract->context, "Unexpected answer checksum.");
return DC_STATUS_PROTOCOL;
}
return DC_STATUS_SUCCESS;
}
static int
shearwater_common_slip_write (shearwater_common_device_t *device, const unsigned char data[], unsigned int size)
{
int n = 0;
const unsigned char end[] = {END};
const unsigned char esc_end[] = {ESC, ESC_END};
const unsigned char esc_esc[] = {ESC, ESC_ESC};
unsigned char buffer[32];
unsigned int nbytes = 0;
#if 0
// Send an initial END character to flush out any data that may have
// accumulated in the receiver due to line noise.
n = device->serial->ops->write (device->serial->port, end, sizeof (end));
if (n != sizeof (end)) {
return EXITCODE(n);
}
#endif
for (unsigned int i = 0; i < size; ++i) {
const unsigned char *seq = NULL;
unsigned int len = 0;
switch (data[i]) {
case END:
// Escape the END character.
seq = esc_end;
len = sizeof (esc_end);
break;
case ESC:
// Escape the ESC character.
seq = esc_esc;
len = sizeof (esc_esc);
break;
default:
// Normal character.
seq = data + i;
len = 1;
break;
}
// Flush the buffer if necessary.
if (nbytes + len + sizeof(end) > sizeof(buffer)) {
n = device->serial->ops->write (device->serial->port, buffer, nbytes);
if (n != nbytes) {
return EXITCODE(n);
}
nbytes = 0;
}
// Append the escaped character.
memcpy(buffer + nbytes, seq, len);
nbytes += len;
}
// Append the END character to indicate the end of the packet.
memcpy(buffer + nbytes, end, sizeof(end));
nbytes += sizeof(end);
// Flush the buffer.
n = device->serial->ops->write (device->serial->port, buffer, nbytes);
if (n != nbytes) {
return EXITCODE(n);
}
return size;
}
