From 6ef72ab4203f8bf6189f0e2c81e9a15b4ae771c5 Mon Sep 17 00:00:00 2001 From: Jef Driesen Date: Wed, 10 Mar 2021 21:38:45 +0100 Subject: [PATCH] Add support for the Sporasub SP2 The Sporasub SP2 uses a very simple communication protocol and memory layout, but with some unusual aspects: Dives are artifically limited to a maximum of 6000 samples. Unlike all other dive computers, the dives are not stored in some kind of ringbuffer structure. Once the memory is full, no new dives can be recorded. The existing dives need to be erased first, and the dive computer will start recording again at te start of the memory area. The Sporasub application has an "Auto-clear watch memory after data transfer" feature for this purpose. I didn't implement a more efficient download algorithm because downloading a full memory dumps takes less than 10 seconds. --- examples/common.c | 1 + include/libdivecomputer/common.h | 2 + msvc/libdivecomputer.vcproj | 12 + src/Makefile.am | 1 + src/descriptor.c | 2 + src/device.c | 4 + src/parser.c | 4 + src/sporasub_sp2.c | 489 +++++++++++++++++++++++++++++++ src/sporasub_sp2.h | 43 +++ src/sporasub_sp2_parser.c | 200 +++++++++++++ 10 files changed, 758 insertions(+) create mode 100644 src/sporasub_sp2.c create mode 100644 src/sporasub_sp2.h create mode 100644 src/sporasub_sp2_parser.c diff --git a/examples/common.c b/examples/common.c index d4e4ad4..73cd1f7 100644 --- a/examples/common.c +++ b/examples/common.c @@ -93,6 +93,7 @@ static const backend_table_t g_backends[] = { {"divecomputereu", DC_FAMILY_TECDIVING_DIVECOMPUTEREU, 0}, {"mclean", DC_FAMILY_MCLEAN_EXTREME, 0}, {"lynx", DC_FAMILY_LIQUIVISION_LYNX, 0}, + {"sp2", DC_FAMILY_SPORASUB_SP2, 0}, }; static const transport_table_t g_transports[] = { diff --git a/include/libdivecomputer/common.h b/include/libdivecomputer/common.h index b8216ac..62f62f3 100644 --- a/include/libdivecomputer/common.h +++ b/include/libdivecomputer/common.h @@ -108,6 +108,8 @@ typedef enum dc_family_t { DC_FAMILY_MCLEAN_EXTREME = (16 << 16), /* Liquivision */ DC_FAMILY_LIQUIVISION_LYNX = (17 << 16), + /* Sporasub */ + DC_FAMILY_SPORASUB_SP2 = (18 << 16), } dc_family_t; #ifdef __cplusplus diff --git a/msvc/libdivecomputer.vcproj b/msvc/libdivecomputer.vcproj index da5187d..3482457 100644 --- a/msvc/libdivecomputer.vcproj +++ b/msvc/libdivecomputer.vcproj @@ -458,6 +458,14 @@ RelativePath="..\src\socket.c" > + + + + @@ -824,6 +832,10 @@ RelativePath="..\src\socket.h" > + + diff --git a/src/Makefile.am b/src/Makefile.am index e78f37d..19bca52 100644 --- a/src/Makefile.am +++ b/src/Makefile.am @@ -74,6 +74,7 @@ libdivecomputer_la_SOURCES = \ tecdiving_divecomputereu.h tecdiving_divecomputereu.c tecdiving_divecomputereu_parser.c \ mclean_extreme.h mclean_extreme.c mclean_extreme_parser.c \ liquivision_lynx.h liquivision_lynx.c liquivision_lynx_parser.c \ + sporasub_sp2.h sporasub_sp2.c sporasub_sp2_parser.c \ socket.h socket.c \ irda.c \ usb.c \ diff --git a/src/descriptor.c b/src/descriptor.c index 531c5d0..5a6d1fe 100644 --- a/src/descriptor.c +++ b/src/descriptor.c @@ -415,6 +415,8 @@ static const dc_descriptor_t g_descriptors[] = { {"Liquivision", "Xeo", DC_FAMILY_LIQUIVISION_LYNX, 1, DC_TRANSPORT_SERIAL, NULL}, {"Liquivision", "Lynx", DC_FAMILY_LIQUIVISION_LYNX, 2, DC_TRANSPORT_SERIAL, NULL}, {"Liquivision", "Kaon", DC_FAMILY_LIQUIVISION_LYNX, 3, DC_TRANSPORT_SERIAL, NULL}, + /* Sporasub */ + {"Sporasub", "SP2", DC_FAMILY_SPORASUB_SP2, 0, DC_TRANSPORT_SERIAL, NULL}, }; static int diff --git a/src/device.c b/src/device.c index 62914dc..6c6732d 100644 --- a/src/device.c +++ b/src/device.c @@ -59,6 +59,7 @@ #include "tecdiving_divecomputereu.h" #include "mclean_extreme.h" #include "liquivision_lynx.h" +#include "sporasub_sp2.h" #include "device-private.h" #include "context-private.h" @@ -219,6 +220,9 @@ dc_device_open (dc_device_t **out, dc_context_t *context, dc_descriptor_t *descr case DC_FAMILY_LIQUIVISION_LYNX: rc = liquivision_lynx_device_open (&device, context, iostream); break; + case DC_FAMILY_SPORASUB_SP2: + rc = sporasub_sp2_device_open (&device, context, iostream); + break; default: return DC_STATUS_INVALIDARGS; } diff --git a/src/parser.c b/src/parser.c index 2f7251d..2596de4 100644 --- a/src/parser.c +++ b/src/parser.c @@ -59,6 +59,7 @@ #include "tecdiving_divecomputereu.h" #include "mclean_extreme.h" #include "liquivision_lynx.h" +#include "sporasub_sp2.h" #include "context-private.h" #include "parser-private.h" @@ -180,6 +181,9 @@ dc_parser_new_internal (dc_parser_t **out, dc_context_t *context, dc_family_t fa case DC_FAMILY_LIQUIVISION_LYNX: rc = liquivision_lynx_parser_create (&parser, context, model); break; + case DC_FAMILY_SPORASUB_SP2: + rc = sporasub_sp2_parser_create (&parser, context); + break; default: return DC_STATUS_INVALIDARGS; } diff --git a/src/sporasub_sp2.c b/src/sporasub_sp2.c new file mode 100644 index 0000000..c0f6b49 --- /dev/null +++ b/src/sporasub_sp2.c @@ -0,0 +1,489 @@ +/* + * libdivecomputer + * + * Copyright (C) 2021 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 // memcpy, memcmp +#include // malloc, free + +#include "sporasub_sp2.h" +#include "context-private.h" +#include "device-private.h" +#include "checksum.h" +#include "array.h" + +#define ISINSTANCE(device) dc_device_isinstance((device), &sporasub_sp2_device_vtable) + +#define SZ_MEMORY 0x10000 + +#define RB_PROFILE_BEGIN 0x0060 +#define RB_PROFILE_END SZ_MEMORY + +#define MAXRETRIES 4 +#define MAXPACKET 256 + +#define HEADER_HI 0xA0 +#define HEADER_LO 0xA2 +#define TRAILER_HI 0xB0 +#define TRAILER_LO 0xB3 + +#define CMD_VERSION 0x10 +#define CMD_READ 0x12 +#define CMD_TIMESYNC 0x39 + +#define SZ_VERSION 23 +#define SZ_READ 128 + +#define SZ_HEADER 32 +#define SZ_SAMPLE 4 + +typedef struct sporasub_sp2_device_t { + dc_device_t base; + dc_iostream_t *iostream; + unsigned char version[SZ_VERSION]; + unsigned char fingerprint[6]; +} sporasub_sp2_device_t; + +static dc_status_t sporasub_sp2_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size); +static dc_status_t sporasub_sp2_device_read (dc_device_t *abstract, unsigned int address, unsigned char data[], unsigned int size); +static dc_status_t sporasub_sp2_device_dump (dc_device_t *abstract, dc_buffer_t *buffer); +static dc_status_t sporasub_sp2_device_foreach (dc_device_t *abstract, dc_dive_callback_t callback, void *userdata); +static dc_status_t sporasub_sp2_device_timesync (dc_device_t *abstract, const dc_datetime_t *datetime); + +static const dc_device_vtable_t sporasub_sp2_device_vtable = { + sizeof(sporasub_sp2_device_t), + DC_FAMILY_SPORASUB_SP2, + sporasub_sp2_device_set_fingerprint, /* set_fingerprint */ + sporasub_sp2_device_read, /* read */ + NULL, /* write */ + sporasub_sp2_device_dump, /* dump */ + sporasub_sp2_device_foreach, /* foreach */ + sporasub_sp2_device_timesync, /* timesync */ + NULL /* close */ +}; + +static unsigned int +iceil (unsigned int x, unsigned int n) +{ + // Round up to next higher multiple. + return ((x + n - 1) / n) * n; +} + +static dc_status_t +sporasub_sp2_send (sporasub_sp2_device_t *device, unsigned char command, const unsigned char data[], unsigned int size) +{ + dc_status_t status = DC_STATUS_SUCCESS; + dc_device_t *abstract = (dc_device_t *) device; + + if (size > MAXPACKET) { + return DC_STATUS_INVALIDARGS; + } + + unsigned int len = size + 1; + unsigned int csum = checksum_add_uint16 (data, size, command); + + unsigned char packet[MAXPACKET + 9] = {0}; + packet[0] = HEADER_HI; + packet[1] = HEADER_LO; + packet[2] = (len >> 8) & 0xFF; + packet[3] = (len ) & 0xFF; + packet[4] = command; + if (size) { + memcpy(packet + 5, data, size); + } + packet[size + 5] = (csum >> 8) & 0xFF; + packet[size + 6] = (csum ) & 0xFF; + packet[size + 7] = TRAILER_HI; + packet[size + 8] = TRAILER_LO; + + // Send the command to the device. + status = dc_iostream_write (device->iostream, packet, size + 9, NULL); + if (status != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to send the command."); + return status; + } + + return DC_STATUS_SUCCESS; +} + +static dc_status_t +sporasub_sp2_receive (sporasub_sp2_device_t *device, unsigned char command, unsigned char data[], unsigned int size) +{ + dc_status_t status = DC_STATUS_SUCCESS; + dc_device_t *abstract = (dc_device_t *) device; + + if (size > MAXPACKET) { + return DC_STATUS_INVALIDARGS; + } + + // Receive the answer of the device. + unsigned char packet[MAXPACKET + 9] = {0}; + status = dc_iostream_read (device->iostream, packet, size + 9, NULL); + if (status != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to receive the answer."); + return status; + } + + // Verify the header and trailer of the packet. + if (packet[0] != HEADER_HI || packet[1] != HEADER_LO || + packet[size + 7] != TRAILER_HI || packet[size + 8] != TRAILER_LO) { + ERROR (abstract->context, "Unexpected answer header/trailer byte."); + return DC_STATUS_PROTOCOL; + } + + // Verify the packet length. + unsigned int len = array_uint16_be (packet + 2); + if (len != size + 1) { + ERROR (abstract->context, "Unexpected packet length."); + return DC_STATUS_PROTOCOL; + } + + // Verify the command byte. + if (packet[4] != command) { + ERROR (abstract->context, "Unexpected answer header/trailer byte."); + return DC_STATUS_PROTOCOL; + } + + // Verify the checksum of the packet. + unsigned short crc = array_uint16_be (packet + size + 5); + unsigned short ccrc = checksum_add_uint16 (packet + 4, size + 1, 0); + if (crc != ccrc) { + ERROR (abstract->context, "Unexpected answer checksum."); + return DC_STATUS_PROTOCOL; + } + + if (size) { + memcpy (data, packet + 5, size); + } + + return DC_STATUS_SUCCESS; +} + +static dc_status_t +sporasub_sp2_packet (sporasub_sp2_device_t *device, unsigned char cmd, const unsigned char command[], unsigned int csize, unsigned char answer[], unsigned int asize) +{ + dc_status_t status = DC_STATUS_SUCCESS; + dc_device_t *abstract = (dc_device_t *) device; + + if (device_is_cancelled (abstract)) + return DC_STATUS_CANCELLED; + + // Send the command to the device. + status = sporasub_sp2_send (device, cmd, command, csize); + if (status != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to send the command."); + return status; + } + + // Receive the answer of the device. + status = sporasub_sp2_receive (device, cmd + 1, answer, asize); + if (status != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to receive the answer."); + return status; + } + + return DC_STATUS_SUCCESS; +} + +static dc_status_t +sporasub_sp2_transfer (sporasub_sp2_device_t *device, unsigned char cmd, const unsigned char command[], unsigned int csize, unsigned char answer[], unsigned int asize) +{ + unsigned int nretries = 0; + dc_status_t rc = DC_STATUS_SUCCESS; + while ((rc = sporasub_sp2_packet (device, cmd, command, csize, answer, asize)) != DC_STATUS_SUCCESS) { + // Automatically discard a corrupted packet, + // and request a new one. + if (rc != DC_STATUS_PROTOCOL && rc != DC_STATUS_TIMEOUT) + return rc; + + // Abort if the maximum number of retries is reached. + if (nretries++ >= MAXRETRIES) + return rc; + + // Discard any garbage bytes. + dc_iostream_sleep (device->iostream, 100); + dc_iostream_purge (device->iostream, DC_DIRECTION_INPUT); + } + + return rc; +} + +dc_status_t +sporasub_sp2_device_open (dc_device_t **out, dc_context_t *context, dc_iostream_t *iostream) +{ + dc_status_t status = DC_STATUS_SUCCESS; + sporasub_sp2_device_t *device = NULL; + + if (out == NULL) + return DC_STATUS_INVALIDARGS; + + // Allocate memory. + device = (sporasub_sp2_device_t *) dc_device_allocate (context, &sporasub_sp2_device_vtable); + if (device == NULL) { + ERROR (context, "Failed to allocate memory."); + return DC_STATUS_NOMEMORY; + } + + // Set the default values. + device->iostream = iostream; + memset (device->fingerprint, 0, sizeof (device->fingerprint)); + + // Set the serial communication protocol (460800 8N1). + status = dc_iostream_configure (device->iostream, 460800, 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_free; + } + + // Set the timeout for receiving data (1000 ms). + status = dc_iostream_set_timeout (device->iostream, 1000); + if (status != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to set the timeout."); + goto error_free; + } + + // Clear the RTS line. + status = dc_iostream_set_rts (device->iostream, 0); + if (status != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to clear the RTS line."); + goto error_free; + } + + // Set the DTR line. + status = dc_iostream_set_dtr (device->iostream, 1); + if (status != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to set the DTR line."); + goto error_free; + } + + dc_iostream_sleep (device->iostream, 100); + dc_iostream_purge (device->iostream, DC_DIRECTION_ALL); + + // Read the version packet. + status = sporasub_sp2_packet(device, CMD_VERSION, NULL, 0, device->version, sizeof(device->version)); + if (status != DC_STATUS_SUCCESS) { + ERROR (context, "Failed to read the version packet."); + goto error_free; + } + + *out = (dc_device_t *) device; + + return DC_STATUS_SUCCESS; + +error_free: + dc_device_deallocate ((dc_device_t *) device); + return status; +} + +static dc_status_t +sporasub_sp2_device_set_fingerprint (dc_device_t *abstract, const unsigned char data[], unsigned int size) +{ + sporasub_sp2_device_t *device = (sporasub_sp2_device_t *) abstract; + + if (size && size != sizeof (device->fingerprint)) + return DC_STATUS_INVALIDARGS; + + if (size) + memcpy (device->fingerprint, data, sizeof (device->fingerprint)); + else + memset (device->fingerprint, 0, sizeof (device->fingerprint)); + + return DC_STATUS_SUCCESS; +} + +static dc_status_t +sporasub_sp2_device_read (dc_device_t *abstract, unsigned int address, unsigned char data[], unsigned int size) +{ + dc_status_t status = DC_STATUS_SUCCESS; + sporasub_sp2_device_t *device = (sporasub_sp2_device_t *) abstract; + + unsigned int nbytes = 0; + while (nbytes < size) { + // Calculate the packet size. + unsigned int len = size - nbytes; + if (len > SZ_READ) + len = SZ_READ; + + // Build the raw command. + unsigned char command[] = { + (address ) & 0xFF, + (address >> 8) & 0xFF, + len}; + + // Send the command and receive the answer. + status = sporasub_sp2_transfer (device, CMD_READ, command, sizeof(command), data + nbytes, len); + if (status != DC_STATUS_SUCCESS) + return status; + + nbytes += len; + address += len; + data += len; + } + + return status; +} + +static dc_status_t +sporasub_sp2_device_dump (dc_device_t *abstract, dc_buffer_t *buffer) +{ + // Allocate the required amount of memory. + if (!dc_buffer_resize (buffer, SZ_MEMORY)) { + ERROR (abstract->context, "Insufficient buffer space available."); + return DC_STATUS_NOMEMORY; + } + + return device_dump_read (abstract, dc_buffer_get_data (buffer), + dc_buffer_get_size (buffer), SZ_READ); +} + +static dc_status_t +sporasub_sp2_device_foreach (dc_device_t *abstract, dc_dive_callback_t callback, void *userdata) +{ + dc_status_t status = DC_STATUS_SUCCESS; + sporasub_sp2_device_t *device = (sporasub_sp2_device_t *) abstract; + + // Emit a device info event. + dc_event_devinfo_t devinfo; + devinfo.model = 0; + devinfo.firmware = 0; + devinfo.serial = array_uint16_be (device->version + 1); + device_event_emit (abstract, DC_EVENT_DEVINFO, &devinfo); + + // Emit a vendor event. + dc_event_vendor_t vendor; + vendor.data = device->version; + vendor.size = sizeof (device->version); + device_event_emit (abstract, DC_EVENT_VENDOR, &vendor); + + dc_buffer_t *buffer = dc_buffer_new (SZ_MEMORY); + if (buffer == NULL) { + status = DC_STATUS_NOMEMORY; + goto error_exit; + } + + status = sporasub_sp2_device_dump (abstract, buffer); + if (status != DC_STATUS_SUCCESS) { + goto error_free_buffer; + } + + unsigned char *data = dc_buffer_get_data (buffer); + + // Get the number of dives. + unsigned int ndives = array_uint16_le (data + 0x02); + + // Get the profile pointer. + unsigned int eop = array_uint16_le (data + 0x04); + if (eop < RB_PROFILE_BEGIN || eop > RB_PROFILE_END) { + ERROR (abstract->context, "Invalid profile pointer (0x%04x).", eop); + status = DC_STATUS_DATAFORMAT; + goto error_free_buffer; + } + + unsigned short *logbook = (unsigned short *) malloc(ndives * sizeof (unsigned short)); + if (logbook == NULL) { + ERROR (abstract->context, "Out of memory."); + status = DC_STATUS_NOMEMORY; + goto error_free_buffer; + } + + // Find all dives. + unsigned int count = 0; + unsigned int address = RB_PROFILE_BEGIN; + while (address + SZ_HEADER <= RB_PROFILE_END && count < ndives) { + if (address == eop) { + WARNING (abstract->context, "Reached end of profile pointer."); + break; + } + + // Get the dive length. + unsigned int nsamples = array_uint16_le (data + address); + unsigned int length = SZ_HEADER + nsamples * SZ_SAMPLE; + if (address + length > RB_PROFILE_END) { + WARNING (abstract->context, "Reached end of memory."); + break; + } + + // Store the address. + logbook[count] = address; + count++; + + // The start of the next dive is always aligned to 32 bytes. + address += iceil (length, SZ_HEADER); + } + + // Process the dives in reverse order (newest first). + for (unsigned int i = 0; i < count; ++i) { + unsigned int idx = count - 1 - i; + unsigned int offset = logbook[idx]; + + // Get the dive length. + unsigned int nsamples = array_uint16_le (data + offset); + unsigned int length = SZ_HEADER + nsamples * SZ_SAMPLE; + + // Check the fingerprint data. + if (memcmp (data + offset + 2, device->fingerprint, sizeof (device->fingerprint)) == 0) + break; + + if (callback && !callback (data + offset, length, data + offset + 2, sizeof (device->fingerprint), userdata)) { + break; + } + } + + free (logbook); +error_free_buffer: + dc_buffer_free (buffer); +error_exit: + return status; +} + + +static dc_status_t +sporasub_sp2_device_timesync (dc_device_t *abstract, const dc_datetime_t *datetime) +{ + dc_status_t status = DC_STATUS_SUCCESS; + sporasub_sp2_device_t *device = (sporasub_sp2_device_t *) abstract; + + if (datetime == NULL || datetime->year < 2000) { + ERROR (abstract->context, "Invalid parameter specified."); + return DC_STATUS_INVALIDARGS; + } + + // Build the raw command. + unsigned char command[] = { + datetime->year - 2000, + datetime->month, + datetime->day, + datetime->hour, + datetime->minute, + datetime->second}; + + // Send the command and receive the answer. + unsigned char answer[1] = {0}; + status = sporasub_sp2_transfer (device, CMD_TIMESYNC, command, sizeof(command), answer, sizeof(answer)); + if (status != DC_STATUS_SUCCESS) + return status; + + // Verify the response code. + if (answer[0] != 0) { + ERROR (abstract->context, "Invalid response code 0x%02x returned.", answer[0]); + return DC_STATUS_PROTOCOL; + } + + return DC_STATUS_SUCCESS; +} diff --git a/src/sporasub_sp2.h b/src/sporasub_sp2.h new file mode 100644 index 0000000..de2ca42 --- /dev/null +++ b/src/sporasub_sp2.h @@ -0,0 +1,43 @@ +/* + * libdivecomputer + * + * Copyright (C) 2021 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 + */ + +#ifndef SPORASUB_SP2_H +#define SPORASUB_SP2_H + +#include +#include +#include +#include + +#ifdef __cplusplus +extern "C" { +#endif /* __cplusplus */ + +dc_status_t +sporasub_sp2_device_open (dc_device_t **device, dc_context_t *context, dc_iostream_t *iostream); + +dc_status_t +sporasub_sp2_parser_create (dc_parser_t **parser, dc_context_t *context); + +#ifdef __cplusplus +} +#endif /* __cplusplus */ +#endif /* SPORASUB_SP2_H */ diff --git a/src/sporasub_sp2_parser.c b/src/sporasub_sp2_parser.c new file mode 100644 index 0000000..e036dd2 --- /dev/null +++ b/src/sporasub_sp2_parser.c @@ -0,0 +1,200 @@ +/* + * libdivecomputer + * + * Copyright (C) 2021 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 + +#include "sporasub_sp2.h" +#include "context-private.h" +#include "parser-private.h" +#include "array.h" + +#define ISINSTANCE(parser) dc_device_isinstance((parser), &sporasub_sp2_parser_vtable) + +#define C_ARRAY_SIZE(array) (sizeof (array) / sizeof *(array)) + +#define SZ_HEADER 0x20 +#define SZ_SAMPLE 0x04 + +typedef struct sporasub_sp2_parser_t sporasub_sp2_parser_t; + +struct sporasub_sp2_parser_t { + dc_parser_t base; +}; + +static dc_status_t sporasub_sp2_parser_set_data (dc_parser_t *abstract, const unsigned char *data, unsigned int size); +static dc_status_t sporasub_sp2_parser_get_datetime (dc_parser_t *abstract, dc_datetime_t *datetime); +static dc_status_t sporasub_sp2_parser_get_field (dc_parser_t *abstract, dc_field_type_t type, unsigned int flags, void *value); +static dc_status_t sporasub_sp2_parser_samples_foreach (dc_parser_t *abstract, dc_sample_callback_t callback, void *userdata); + +static const dc_parser_vtable_t sporasub_sp2_parser_vtable = { + sizeof(sporasub_sp2_parser_t), + DC_FAMILY_SPORASUB_SP2, + sporasub_sp2_parser_set_data, /* set_data */ + sporasub_sp2_parser_get_datetime, /* datetime */ + sporasub_sp2_parser_get_field, /* fields */ + sporasub_sp2_parser_samples_foreach, /* samples_foreach */ + NULL /* destroy */ +}; + + +dc_status_t +sporasub_sp2_parser_create (dc_parser_t **out, dc_context_t *context) +{ + sporasub_sp2_parser_t *parser = NULL; + + if (out == NULL) + return DC_STATUS_INVALIDARGS; + + // Allocate memory. + parser = (sporasub_sp2_parser_t *) dc_parser_allocate (context, &sporasub_sp2_parser_vtable); + if (parser == NULL) { + ERROR (context, "Failed to allocate memory."); + return DC_STATUS_NOMEMORY; + } + + *out = (dc_parser_t *) parser; + + return DC_STATUS_SUCCESS; +} + + +static dc_status_t +sporasub_sp2_parser_set_data (dc_parser_t *abstract, const unsigned char *data, unsigned int size) +{ + return DC_STATUS_SUCCESS; +} + + +static dc_status_t +sporasub_sp2_parser_get_datetime (dc_parser_t *abstract, dc_datetime_t *datetime) +{ + const unsigned char *data = abstract->data; + unsigned int size = abstract->size; + + if (size < SZ_HEADER) + return DC_STATUS_DATAFORMAT; + + if (datetime) { + datetime->year = data[4] + 2000; + datetime->month = data[3]; + datetime->day = data[2]; + datetime->hour = data[7]; + datetime->minute = data[6]; + datetime->second = data[5]; + datetime->timezone = DC_TIMEZONE_NONE; + } + + return DC_STATUS_SUCCESS; +} + + +static dc_status_t +sporasub_sp2_parser_get_field (dc_parser_t *abstract, dc_field_type_t type, unsigned int flags, void *value) +{ + const unsigned char *data = abstract->data; + unsigned int size = abstract->size; + + if (size < SZ_HEADER) + return DC_STATUS_DATAFORMAT; + + if (value) { + switch (type) { + case DC_FIELD_DIVETIME: + *((unsigned int *) value) = data[0x08] + data[0x09] * 60; + break; + case DC_FIELD_MAXDEPTH: + *((double *) value) = array_uint16_le (data + 0x14) / 100.0; + break; + case DC_FIELD_DIVEMODE: + *((dc_divemode_t *) value) = DC_DIVEMODE_FREEDIVE; + break; + case DC_FIELD_TEMPERATURE_MINIMUM: + *((double *) value) = array_uint16_le (data + 0x18) / 10.0; + break; + case DC_FIELD_TEMPERATURE_MAXIMUM: + *((double *) value) = array_uint16_le (data + 0x16) / 10.0; + break; + default: + return DC_STATUS_UNSUPPORTED; + } + } + + return DC_STATUS_SUCCESS; +} + + +static dc_status_t +sporasub_sp2_parser_samples_foreach (dc_parser_t *abstract, dc_sample_callback_t callback, void *userdata) +{ + const unsigned char *data = abstract->data; + unsigned int size = abstract->size; + + if (size < SZ_HEADER) + return DC_STATUS_DATAFORMAT; + + unsigned int nsamples = array_uint16_le(data); + + // Get the sample interval. + unsigned int interval_idx = data[0x1A]; + const unsigned int intervals[] = {1, 2, 5, 10}; + if (interval_idx >= C_ARRAY_SIZE(intervals)) { + ERROR (abstract->context, "Invalid sample interval index %u", interval_idx); + return DC_STATUS_DATAFORMAT; + } + unsigned int interval = intervals[interval_idx]; + + unsigned int time = 0; + unsigned int count = 0; + unsigned int offset = SZ_HEADER; + while (offset + SZ_SAMPLE <= size && count < nsamples) { + dc_sample_value_t sample = {0}; + + unsigned int value = array_uint32_le (data + offset); + unsigned int heartrate = (value & 0xFF000000) >> 24; + unsigned int temperature = (value & 0x00FFC000) >> 14; + unsigned int unknown = (value & 0x00003000) >> 12; + unsigned int depth = (value & 0x00000FFF) >> 0; + + // Time (seconds) + time += interval; + sample.time = time; + if (callback) callback (DC_SAMPLE_TIME, sample, userdata); + + // Depth (1/100 m) + sample.depth = depth / 100.0; + if (callback) callback (DC_SAMPLE_DEPTH, sample, userdata); + + // Temperature (1/10 °C) + sample.temperature = temperature / 10.0 - 20.0; + if (callback) callback (DC_SAMPLE_TEMPERATURE, sample, userdata); + + // Heartrate + if (heartrate) { + sample.heartbeat = heartrate; + if (callback) callback (DC_SAMPLE_HEARTBEAT, sample, userdata); + } + + offset += SZ_SAMPLE; + count++; + } + + return DC_STATUS_SUCCESS; +}