FlapJackApps/libdivecomputer
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Simplified the error recovery by moving the code to a separate function.

Browse Source
This commit is contained in:
Jef Driesen 2008-05-20 08:59:15 +00:00
parent 9c30a72683
commit 857cd39c54
1 changed files with 124 additions and 78 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

202
uwatec_memomouse.c
View File

@ -1,5 +1,6 @@
#include <string.h> // memcmp, memcpy
#include <stdlib.h> // malloc, free
#include <assert.h> // assert
#include "uwatec.h"
#include "serial.h"
@ -157,123 +158,167 @@ serial_write_uwatec (serial *device, const unsigned char data[], unsigned int si
static int
uwatec_memomouse_read_packet_outer (memomouse *device, unsigned char data[], unsigned int size)
uwatec_memomouse_confirm (memomouse *device, unsigned char value)
{
// Initial checksum value.
unsigned char ccrc = 0x00;
// Send the value to the device.
int rc = serial_write_uwatec (device->port, &value, 1);
if (rc != 1) {
WARNING ("Failed to send the value.");
return EXITCODE (rc, 1);
}
return UWATEC_SUCCESS;
}
static int
uwatec_memomouse_read_packet (memomouse *device, unsigned char data[], unsigned int size)
{
assert (size >= 126 + 2);
// Receive the header of the package.
unsigned char len = 0x00;
int rc = serial_read_uwatec (device->port, &len, 1);
if (rc != 1 || len > size) {
int rc = serial_read (device->port, data, 1);
if (rc != 1) {
WARNING ("Failed to receive the answer.");
return EXITCODE (rc, 1);
}
// Reverse the bits.
uwatec_memomouse_reverse (data, 1);
// Verify the header of the package.
unsigned int len = data[0];
if (len > 126) {
WARNING ("Unexpected answer start byte(s).");
return EXITCODE (rc, 1);
return UWATEC_ERROR_PROTOCOL;
}
// Update the checksum.
ccrc = uwatec_memomouse_checksum (&len, 1, ccrc);
// Receive the contents of the package.
rc = serial_read_uwatec (device->port, data, len);
if (rc != len) {
WARNING ("Unexpected EOF in answer.");
return EXITCODE (rc, len);
// Receive the remaining part of the package.
rc = serial_read (device->port, data + 1, len + 1);
if (rc != len + 1) {
WARNING ("Failed to receive the answer.");
return EXITCODE (rc, len + 1);
}
// Update the checksum.
ccrc = uwatec_memomouse_checksum (data, len, ccrc);
// Receive (and verify) the checksum of the package.
unsigned char crc = 0x00;
rc = serial_read_uwatec (device->port, &crc, 1);
if (rc != 1 || ccrc != crc) {
// Reverse the bits.
uwatec_memomouse_reverse (data + 1, len + 1);
// Verify the checksum of the package.
unsigned char crc = data[len + 1];
unsigned char ccrc = uwatec_memomouse_checksum (data, len + 1, 0x00);
if (crc != ccrc) {
WARNING ("Unexpected answer CRC.");
return EXITCODE (rc, 1);
return UWATEC_ERROR_PROTOCOL;
}
#ifndef NDEBUG
message ("package(%i)=\"", len);
for (unsigned int i = 0; i < len; ++i) {
message ("%02x", data[i]);
}
message ("\"\n");
#endif
return len;
}
static int
uwatec_memomouse_read_packet_inner (memomouse *device, unsigned char data[], unsigned int msize)
uwatec_memomouse_read_packet_outer (memomouse *device, unsigned char data[], unsigned int size)
{
int rc = 0;
unsigned char package[126 + 2] = {0};
while ((rc = uwatec_memomouse_read_packet (device, package, sizeof (package))) < 0) {
// Automatically discard a corrupted packet,
// and request a new one.
if (rc != UWATEC_ERROR_PROTOCOL)
return rc;
// Flush the input buffer.
serial_flush (device->port, SERIAL_QUEUE_INPUT);
// Reject the packet.
rc = uwatec_memomouse_confirm (device, nak);
if (rc != UWATEC_SUCCESS)
return rc;
}
#ifndef NDEBUG
message ("package(%i)=\"", rc);
for (unsigned int i = 0; i < rc; ++i) {
message ("%02x", package[i + 1]);
}
message ("\"\n");
#endif
if (size >= rc)
memcpy (data, package + 1, rc);
else
WARNING ("Insufficient buffer space available.");
return rc;
}
static int
uwatec_memomouse_read_packet_inner (memomouse *device, unsigned char data[], unsigned int size)
{
// Initial checksum value.
unsigned char ccrc = 0x00;
// Read the first package.
int rc = 0;
unsigned char package[128 - 2] = {0};
for (;;) {
rc = uwatec_memomouse_read_packet_outer (device, package, sizeof (package));
if (rc < 0) {
message ("Didn't got expected packet, sending NAK\n");
serial_flush (device->port, SERIAL_QUEUE_INPUT);
serial_write_uwatec (device->port, &nak, 1);
message ("Package: nbytes=%u, rc=%i\n", 0, rc);
} else {
message ("Got expected packet, sending ACK\n");
serial_write_uwatec (device->port, &ack, 1);
break;
}
}
unsigned char package[126] = {0};
int rca = uwatec_memomouse_read_packet_outer (device, package, sizeof (package));
if (rca < 0)
return rca;
if (rc < 2) {
message ("Package too small.\n");
return UWATEC_ERROR;
// Accept the package.
int rcb = uwatec_memomouse_confirm (device, ack);
if (rcb != UWATEC_SUCCESS)
return rcb;
if (rca < 2) {
message ("First package is too small.\n");
return UWATEC_ERROR_PROTOCOL;
}
// Calculate the total size of the inner package.
unsigned int size = package[0] + (package[1] << 8) + 3;
message ("Package: size=%u\n", size);
unsigned int total = package[0] + (package[1] << 8) + 3;
message ("Package: size=%u\n", total);
// Calculate the size of the data in current package
// (excluding the checksum).
unsigned int len = (rc >= size ? size - 1 : rc);
message ("Package: nbytes=%u, rc=%i, len=%u\n", 0, rc, len);
unsigned int len = (rca >= total ? total - 1 : rca);
message ("Package: nbytes=%u, rc=%i, len=%u\n", 0, rca, len);
// Update the checksum.
ccrc = uwatec_memomouse_checksum (package, len, ccrc);
// Append the data package to the output buffer.
if (len - 2 <= msize)
if (len - 2 <= size)
memcpy (data, package + 2, len - 2);
else
message ("Insufficient buffer space available.\n");
unsigned int nbytes = rc;
while (nbytes < size) {
rc = uwatec_memomouse_read_packet_outer (device, package, sizeof (package));
if (rc < 0) {
message ("Didn't got expected packet, sending NAK\n");
serial_flush (device->port, SERIAL_QUEUE_INPUT);
serial_write_uwatec (device->port, &nak, 1);
message ("Package: nbytes=%u, rc=%i\n", nbytes, rc);
} else {
message ("Got expected packet, sending ACK\n");
serial_write_uwatec (device->port, &ack, 1);
unsigned int nbytes = rca;
while (nbytes < total) {
// Read the package.
rca = uwatec_memomouse_read_packet_outer (device, package, sizeof (package));
if (rca < 0)
return rca;
// Calculate the size of the data in current package
// (excluding the checksum).
len = (nbytes + rc >= size ? size - nbytes - 1 : rc);
message ("Package: nbytes=%u, rc=%u, len=%u\n", nbytes, rc, len);
// Accept the package.
rcb = uwatec_memomouse_confirm (device, ack);
if (rcb != UWATEC_SUCCESS)
return rcb;
// Update the checksum.
ccrc = uwatec_memomouse_checksum (package, len, ccrc);
// Calculate the size of the data in current package
// (excluding the checksum).
len = (nbytes + rca >= total ? total - nbytes - 1 : rca);
message ("Package: nbytes=%u, rc=%u, len=%u\n", nbytes, rca, len);
// Append the data package to the output buffer.
if (nbytes + len - 2 <= msize)
memcpy (data + nbytes - 2, package, len);
else
message ("Insufficient buffer space available.\n");
// Update the checksum.
ccrc = uwatec_memomouse_checksum (package, len, ccrc);
nbytes += rc;
}
// Append the data package to the output buffer.
if (nbytes + len - 2 <= size)
memcpy (data + nbytes - 2, package, len);
else
message ("Insufficient buffer space available.\n");
nbytes += rca;
}
message ("Package: nbytes=%i\n", nbytes);
@ -286,6 +331,7 @@ uwatec_memomouse_read_packet_inner (memomouse *device, unsigned char data[], uns
return UWATEC_SUCCESS;
}
int
uwatec_memomouse_read (memomouse *device, unsigned char data[], unsigned int size)
{
@ -321,7 +367,7 @@ uwatec_memomouse_read (memomouse *device, unsigned char data[], unsigned int siz
while (answer != ack) {
message ("Sending command\n");
serial_flush (device->port, SERIAL_QUEUE_INPUT);
rc = serial_write (device->port, command, sizeof (command));
rc = serial_write_uwatec (device->port, command, sizeof (command));
if (rc != sizeof (command)) {
WARNING ("Failed to send command");
return EXITCODE (rc, sizeof (command));