20d268acdb
This is just to make future merges easier. There were nothing left of Claudiu's code in those files, so thats why i removed the copyright lines. Signed-off-by: Anton Lundin <glance@acc.umu.se> Signed-off-by: Dirk Hohndel <dirk@hohndel.org>
502 lines
14 KiB
C
502 lines
14 KiB
C
/*
|
||
* libdivecomputer
|
||
*
|
||
* Copyright (C) 2013 Jef Driesen
|
||
*
|
||
* This library is free software; you can redistribute it and/or
|
||
* modify it under the terms of the GNU Lesser General Public
|
||
* License as published by the Free Software Foundation; either
|
||
* version 2.1 of the License, or (at your option) any later version.
|
||
*
|
||
* This library is distributed in the hope that it will be useful,
|
||
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
|
||
* Lesser General Public License for more details.
|
||
*
|
||
* You should have received a copy of the GNU Lesser General Public
|
||
* License along with this library; if not, write to the Free Software
|
||
* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston,
|
||
* MA 02110-1301 USA
|
||
*/
|
||
|
||
#include <string.h> // memcmp, memcpy
|
||
#include <stdlib.h> // malloc, free
|
||
|
||
#include "shearwater_common.h"
|
||
|
||
#include "context-private.h"
|
||
#include "array.h"
|
||
|
||
#define SZ_PACKET 254
|
||
|
||
// SLIP special character codes
|
||
#define END 0xC0
|
||
#define ESC 0xDB
|
||
#define ESC_END 0xDC
|
||
#define ESC_ESC 0xDD
|
||
|
||
dc_status_t
|
||
shearwater_common_open (shearwater_common_device_t *device, dc_context_t *context, const char *name)
|
||
{
|
||
dc_status_t status = DC_STATUS_SUCCESS;
|
||
|
||
// Open the device.
|
||
status = dc_serial_open (&device->port, context, name);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
ERROR (context, "Failed to open the serial port.");
|
||
return status;
|
||
}
|
||
|
||
// Set the serial communication protocol (115200 8N1).
|
||
status = dc_serial_configure (device->port, 115200, 8, DC_PARITY_NONE, DC_STOPBITS_ONE, DC_FLOWCONTROL_NONE);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
ERROR (context, "Failed to set the terminal attributes.");
|
||
goto error_close;
|
||
}
|
||
|
||
// Set the timeout for receiving data (3000ms).
|
||
status = dc_serial_set_timeout (device->port, 3000);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
ERROR (context, "Failed to set the timeout.");
|
||
status = DC_STATUS_IO;
|
||
goto error_close;
|
||
}
|
||
|
||
// Make sure everything is in a sane state.
|
||
dc_serial_sleep (device->port, 300);
|
||
dc_serial_purge (device->port, DC_DIRECTION_ALL);
|
||
|
||
return DC_STATUS_SUCCESS;
|
||
|
||
error_close:
|
||
dc_serial_close (device->port);
|
||
return status;
|
||
}
|
||
|
||
|
||
dc_status_t
|
||
shearwater_common_close (shearwater_common_device_t *device)
|
||
{
|
||
// Close the device.
|
||
return dc_serial_close (device->port);
|
||
}
|
||
|
||
|
||
static int
|
||
shearwater_common_decompress_lre (unsigned char *data, unsigned int size, dc_buffer_t *buffer, unsigned int *isfinal)
|
||
{
|
||
// The RLE decompression algorithm does interpret the binary data as a
|
||
// stream of 9 bit values. Therefore, the total number of bits needs to be
|
||
// a multiple of 9 bits.
|
||
unsigned int nbits = size * 8;
|
||
if (nbits % 9 != 0)
|
||
return -1;
|
||
|
||
unsigned int offset = 0;
|
||
while (offset + 9 <= nbits) {
|
||
// Extract the 9 bit value.
|
||
unsigned int byte = offset / 8;
|
||
unsigned int bit = offset % 8;
|
||
unsigned int shift = 16 - (bit + 9);
|
||
unsigned int value = (array_uint16_be (data + byte) >> shift) & 0x1FF;
|
||
|
||
// The 9th bit indicates whether the remaining 8 bits represent
|
||
// a run of zero bytes or not. If the bit is set, the value is
|
||
// not a run and doesn’t need expansion. If the bit is not set,
|
||
// the value contains the number of zero bytes in the run. A
|
||
// zero-length run indicates the end of the compressed stream.
|
||
if (value & 0x100) {
|
||
// Append the data byte directly.
|
||
unsigned char c = value & 0xFF;
|
||
if (!dc_buffer_append (buffer, &c, 1))
|
||
return -1;
|
||
} else if (value == 0) {
|
||
// Reached the end of the compressed stream.
|
||
if (isfinal)
|
||
*isfinal = 1;
|
||
break;
|
||
} else {
|
||
// Expand the run with zero bytes.
|
||
if (!dc_buffer_resize (buffer, dc_buffer_get_size (buffer) + value))
|
||
return -1;
|
||
}
|
||
|
||
offset += 9;
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
static int
|
||
shearwater_common_decompress_xor (unsigned char *data, unsigned int size)
|
||
{
|
||
// Each block of 32 bytes is XOR'ed (in-place) with the previous block,
|
||
// except for the first block, which is passed through unchanged.
|
||
for (unsigned int i = 32; i < size; ++i) {
|
||
data[i] ^= data[i - 32];
|
||
}
|
||
|
||
return 0;
|
||
}
|
||
|
||
|
||
static dc_status_t
|
||
shearwater_common_slip_write (shearwater_common_device_t *device, const unsigned char data[], unsigned int size)
|
||
{
|
||
dc_status_t status = DC_STATUS_SUCCESS;
|
||
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.
|
||
status = dc_serial_write (device->port, end, sizeof (end), NULL);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
return status;
|
||
}
|
||
#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)) {
|
||
status = dc_serial_write (device->port, buffer, nbytes, NULL);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
return status;
|
||
}
|
||
|
||
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.
|
||
status = dc_serial_write (device->port, buffer, nbytes, NULL);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
return status;
|
||
}
|
||
|
||
return DC_STATUS_SUCCESS;
|
||
}
|
||
|
||
|
||
static dc_status_t
|
||
shearwater_common_slip_read (shearwater_common_device_t *device, unsigned char data[], unsigned int size, unsigned int *actual)
|
||
{
|
||
dc_status_t status = DC_STATUS_SUCCESS;
|
||
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;
|
||
|
||
// Get a single character to process.
|
||
status = dc_serial_read (device->port, &c, 1, NULL);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
return status;
|
||
}
|
||
|
||
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)
|
||
goto done;
|
||
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.
|
||
status = dc_serial_read (device->port, &c, 1, NULL);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
return status;
|
||
}
|
||
|
||
// 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++;
|
||
}
|
||
}
|
||
|
||
done:
|
||
|
||
if (received > size)
|
||
return DC_STATUS_PROTOCOL;
|
||
|
||
if (actual)
|
||
*actual = received;
|
||
|
||
return DC_STATUS_SUCCESS;
|
||
}
|
||
|
||
|
||
dc_status_t
|
||
shearwater_common_transfer (shearwater_common_device_t *device, const unsigned char input[], unsigned int isize, unsigned char output[], unsigned int osize, unsigned int *actual)
|
||
{
|
||
dc_status_t status = DC_STATUS_SUCCESS;
|
||
dc_device_t *abstract = (dc_device_t *) device;
|
||
unsigned char packet[SZ_PACKET + 4];
|
||
unsigned int n = 0;
|
||
|
||
if (isize > SZ_PACKET || osize > SZ_PACKET)
|
||
return DC_STATUS_INVALIDARGS;
|
||
|
||
if (device_is_cancelled (abstract))
|
||
return DC_STATUS_CANCELLED;
|
||
|
||
// Setup the request packet.
|
||
packet[0] = 0xFF;
|
||
packet[1] = 0x01;
|
||
packet[2] = isize + 1;
|
||
packet[3] = 0x00;
|
||
memcpy (packet + 4, input, isize);
|
||
|
||
// Send the request packet.
|
||
status = shearwater_common_slip_write (device, packet, isize + 4);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
ERROR (abstract->context, "Failed to send the request packet.");
|
||
return status;
|
||
}
|
||
|
||
// Return early if no response packet is requested.
|
||
if (osize == 0) {
|
||
if (actual)
|
||
*actual = 0;
|
||
return DC_STATUS_SUCCESS;
|
||
}
|
||
|
||
// Receive the response packet.
|
||
status = shearwater_common_slip_read (device, packet, sizeof (packet), &n);
|
||
if (status != DC_STATUS_SUCCESS) {
|
||
ERROR (abstract->context, "Failed to receive the response packet.");
|
||
return status;
|
||
}
|
||
|
||
// Validate the packet header.
|
||
if (n < 4 || packet[0] != 0x01 || packet[1] != 0xFF || packet[3] != 0x00) {
|
||
ERROR (abstract->context, "Invalid packet header.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
// Validate the packet length.
|
||
unsigned int length = packet[2];
|
||
if (length < 1 || length - 1 + 4 != n || length - 1 > osize) {
|
||
ERROR (abstract->context, "Invalid packet header.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
memcpy (output, packet + 4, length - 1);
|
||
if (actual)
|
||
*actual = length - 1;
|
||
|
||
return DC_STATUS_SUCCESS;
|
||
}
|
||
|
||
|
||
dc_status_t
|
||
shearwater_common_download (shearwater_common_device_t *device, dc_buffer_t *buffer, unsigned int address, unsigned int size, unsigned int compression)
|
||
{
|
||
dc_device_t *abstract = (dc_device_t *) device;
|
||
dc_status_t rc = DC_STATUS_SUCCESS;
|
||
unsigned int n = 0;
|
||
|
||
unsigned char req_init[] = {
|
||
0x35,
|
||
(compression ? 0x10 : 0x00),
|
||
0x34,
|
||
(address >> 24) & 0xFF,
|
||
(address >> 16) & 0xFF,
|
||
(address >> 8) & 0xFF,
|
||
(address ) & 0xFF,
|
||
(size >> 16) & 0xFF,
|
||
(size >> 8) & 0xFF,
|
||
(size ) & 0xFF};
|
||
unsigned char req_block[] = {0x36, 0x00};
|
||
unsigned char req_quit[] = {0x37};
|
||
unsigned char response[SZ_PACKET];
|
||
|
||
// 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 = 3 + size + 1;
|
||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||
|
||
// Transfer the init request.
|
||
rc = shearwater_common_transfer (device, req_init, sizeof (req_init), response, 3, &n);
|
||
if (rc != DC_STATUS_SUCCESS) {
|
||
return rc;
|
||
}
|
||
|
||
// Verify the init response.
|
||
if (n != 3 || response[0] != 0x75 || response[1] != 0x10 || response[2] > SZ_PACKET) {
|
||
ERROR (abstract->context, "Unexpected response packet.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
// Update and emit a progress event.
|
||
progress.current += 3;
|
||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||
|
||
unsigned int done = 0;
|
||
unsigned char block = 1;
|
||
unsigned int nbytes = 0;
|
||
while (nbytes < size && !done) {
|
||
// Transfer the block request.
|
||
req_block[1] = block;
|
||
rc = shearwater_common_transfer (device, req_block, sizeof (req_block), response, sizeof (response), &n);
|
||
if (rc != DC_STATUS_SUCCESS) {
|
||
return rc;
|
||
}
|
||
|
||
// Verify the block header.
|
||
if (n < 2 || response[0] != 0x76 || response[1] != block) {
|
||
ERROR (abstract->context, "Unexpected response packet.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
// Verify the block length.
|
||
unsigned int length = n - 2;
|
||
if (nbytes + length > size) {
|
||
ERROR (abstract->context, "Unexpected packet size.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
// Update and emit a progress event.
|
||
progress.current += length;
|
||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||
|
||
if (compression) {
|
||
if (shearwater_common_decompress_lre (response + 2, length, buffer, &done) != 0) {
|
||
ERROR (abstract->context, "Decompression error (LRE phase).");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
} else {
|
||
if (!dc_buffer_append (buffer, response + 2, length)) {
|
||
ERROR (abstract->context, "Insufficient buffer space available.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
}
|
||
|
||
nbytes += length;
|
||
block++;
|
||
}
|
||
|
||
if (compression) {
|
||
if (shearwater_common_decompress_xor (dc_buffer_get_data (buffer), dc_buffer_get_size (buffer)) != 0) {
|
||
ERROR (abstract->context, "Decompression error (XOR phase).");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
}
|
||
|
||
// Transfer the quit request.
|
||
rc = shearwater_common_transfer (device, req_quit, sizeof (req_quit), response, 2, &n);
|
||
if (rc != DC_STATUS_SUCCESS) {
|
||
return rc;
|
||
}
|
||
|
||
// Verify the quit response.
|
||
if (n != 2 || response[0] != 0x77 || response[1] != 0x00) {
|
||
ERROR (abstract->context, "Unexpected response packet.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
// Update and emit a progress event.
|
||
progress.current += 1;
|
||
device_event_emit (abstract, DC_EVENT_PROGRESS, &progress);
|
||
|
||
return DC_STATUS_SUCCESS;
|
||
}
|
||
|
||
|
||
dc_status_t
|
||
shearwater_common_identifier (shearwater_common_device_t *device, dc_buffer_t *buffer, unsigned int id)
|
||
{
|
||
dc_device_t *abstract = (dc_device_t *) device;
|
||
dc_status_t rc = DC_STATUS_SUCCESS;
|
||
|
||
// Erase the buffer.
|
||
if (!dc_buffer_clear (buffer)) {
|
||
ERROR (abstract->context, "Insufficient buffer space available.");
|
||
return DC_STATUS_NOMEMORY;
|
||
}
|
||
|
||
// Transfer the request.
|
||
unsigned int n = 0;
|
||
unsigned char request[] = {0x22,
|
||
(id >> 8) & 0xFF,
|
||
(id ) & 0xFF};
|
||
unsigned char response[SZ_PACKET];
|
||
rc = shearwater_common_transfer (device, request, sizeof (request), response, sizeof (response), &n);
|
||
if (rc != DC_STATUS_SUCCESS) {
|
||
return rc;
|
||
}
|
||
|
||
// Verify the response.
|
||
if (n < 3 || response[0] != 0x62 || response[1] != request[1] || response[2] != request[2]) {
|
||
ERROR (abstract->context, "Unexpected response packet.");
|
||
return DC_STATUS_PROTOCOL;
|
||
}
|
||
|
||
// Append the packet to the output buffer.
|
||
if (!dc_buffer_append (buffer, response + 3, n - 3)) {
|
||
ERROR (abstract->context, "Insufficient buffer space available.");
|
||
return DC_STATUS_NOMEMORY;
|
||
}
|
||
|
||
return rc;
|
||
}
|