diff --git a/src/shearwater_common.c b/src/shearwater_common.c index f3cdc48..8e4f493 100644 --- a/src/shearwater_common.c +++ b/src/shearwater_common.c @@ -441,3 +441,42 @@ shearwater_common_download (shearwater_common_device_t *device, dc_buffer_t *buf 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; +} diff --git a/src/shearwater_common.h b/src/shearwater_common.h index dbb717e..a07de14 100644 --- a/src/shearwater_common.h +++ b/src/shearwater_common.h @@ -29,6 +29,9 @@ extern "C" { #endif /* __cplusplus */ +#define ID_SERIAL 0x8010 +#define ID_FIRMWARE 0x8011 + typedef struct shearwater_common_device_t { dc_device_t base; serial_t *port; @@ -46,6 +49,9 @@ shearwater_common_transfer (shearwater_common_device_t *device, const unsigned c 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_status_t +shearwater_common_identifier (shearwater_common_device_t *device, dc_buffer_t *buffer, unsigned int id); + #ifdef __cplusplus } #endif /* __cplusplus */ diff --git a/src/shearwater_petrel.c b/src/shearwater_petrel.c index ac84848..3534a26 100644 --- a/src/shearwater_petrel.c +++ b/src/shearwater_petrel.c @@ -61,6 +61,21 @@ static const dc_device_vtable_t shearwater_petrel_device_vtable = { }; +static unsigned int +str2num (unsigned char data[], unsigned int size, unsigned int offset) +{ + unsigned int value = 0; + for (unsigned int i = offset; i < size; ++i) { + if (data[i] < '0' || data[i] > '9') + break; + value *= 10; + value += data[i] - '0'; + } + + return value; +} + + dc_status_t shearwater_petrel_device_open (dc_device_t **out, dc_context_t *context, const char *name) { @@ -148,6 +163,45 @@ shearwater_petrel_device_foreach (dc_device_t *abstract, dc_dive_callback_t call return DC_STATUS_NOMEMORY; } + // Read the serial number. + rc = shearwater_common_identifier (&device->base, buffer, ID_SERIAL); + if (rc != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to read the serial number."); + dc_buffer_free (buffer); + dc_buffer_free (manifests); + return rc; + } + + // Convert to a number. + unsigned char serial[4] = {0}; + if (array_convert_hex2bin (dc_buffer_get_data (buffer), dc_buffer_get_size (buffer), + serial, sizeof (serial)) != 0 ) { + ERROR (abstract->context, "Failed to convert the serial number."); + dc_buffer_free (buffer); + dc_buffer_free (manifests); + return DC_STATUS_DATAFORMAT; + + } + + // Read the firmware version. + rc = shearwater_common_identifier (&device->base, buffer, ID_FIRMWARE); + if (rc != DC_STATUS_SUCCESS) { + ERROR (abstract->context, "Failed to read the firmware version."); + dc_buffer_free (buffer); + dc_buffer_free (manifests); + return rc; + } + + // Convert to a number. + unsigned int firmware = str2num (dc_buffer_get_data (buffer), dc_buffer_get_size (buffer), 1); + + // Emit a device info event. + dc_event_devinfo_t devinfo; + devinfo.model = 3; + devinfo.firmware = firmware; + devinfo.serial = array_uint32_be (serial); + device_event_emit (abstract, DC_EVENT_DEVINFO, &devinfo); + while (1) { // Download a manifest. rc = shearwater_common_download (&device->base, buffer, MANIFEST_ADDR, MANIFEST_SIZE, 0);