75 lines
1.9 KiB
C
75 lines
1.9 KiB
C
#include <assert.h>
|
|
#include <string.h>
|
|
|
|
#include "suunto_common.h"
|
|
|
|
#define SUBSTRACT(a, x, begin, end) substract (a - begin, x, end - begin) + begin
|
|
#define DISTANCE(a, b, begin, end) distance (a, b, end - begin)
|
|
|
|
static unsigned int
|
|
substract (unsigned int a, unsigned int x, unsigned int size)
|
|
{
|
|
if (x <= a) {
|
|
return (a - x) % size;
|
|
} else {
|
|
return size - (x - a) % size;
|
|
}
|
|
}
|
|
|
|
static unsigned int
|
|
distance (unsigned int a, unsigned int b, unsigned int size)
|
|
{
|
|
if (a <= b) {
|
|
return (b - a) % size;
|
|
} else {
|
|
return size - (a - b) % size;
|
|
}
|
|
}
|
|
|
|
int
|
|
suunto_common_extract_dives (const unsigned char data[], unsigned int begin, unsigned int end, unsigned int eop, unsigned int peek, dive_callback_t callback, void *userdata)
|
|
{
|
|
assert (eop >= begin && eop < end);
|
|
assert (data[eop] == 0x82);
|
|
|
|
unsigned char buffer[0x2000 - 0x4C] = {0};
|
|
assert (sizeof (buffer) >= end - begin);
|
|
|
|
unsigned int current = eop;
|
|
unsigned int previous = eop;
|
|
for (unsigned int i = 0; i < end - begin; ++i) {
|
|
// Move backwards through the ringbuffer.
|
|
if (current == begin)
|
|
current = end;
|
|
current--;
|
|
|
|
// Check for an end of profile marker.
|
|
if (data[current] == 0x82)
|
|
break;
|
|
|
|
// Check for an end of dive marker (of the next dive),
|
|
// to find the start of the current dive.
|
|
unsigned int peek = SUBSTRACT (current, peek, begin, end);
|
|
if (data[peek] == 0x80) {
|
|
unsigned int len = DISTANCE (current, previous, begin, end);
|
|
if (current + len > end) {
|
|
unsigned int a = end - current;
|
|
unsigned int b = (current + len) - end;
|
|
memcpy (buffer + 0, data + current, a);
|
|
memcpy (buffer + a, data + begin, b);
|
|
} else {
|
|
memcpy (buffer, data + current, len);
|
|
}
|
|
|
|
if (callback)
|
|
callback (buffer, len, userdata);
|
|
|
|
previous = current;
|
|
}
|
|
}
|
|
|
|
assert (data[current] == 0x82);
|
|
|
|
return SUUNTO_SUCCESS;
|
|
}
|