int libambit_pmem20_log_next_header(ambit_object_t *object, ambit_log_header_t *log_header)
{
int ret = -1;
size_t buffer_offset;
uint16_t tmp_len;
LOG_INFO("Reading header of next log entry");
if (!object->pmem20.log.initialized) {
LOG_ERROR("Trying to get next log without initialization");
return -1;
}
// Check if we reached end of entries
if (object->pmem20.log.current.current == object->pmem20.log.current.next) {
LOG_INFO("No more entries to read");
return 0;
}
if (read_upto(object, object->pmem20.log.current.next, PMEM20_LOG_HEADER_MIN_LEN) == 0) {
buffer_offset = (object->pmem20.log.current.next - PMEM20_LOG_START);
// First check that header seems to be correctly present
if (strncmp((char*)object->pmem20.log.buffer + buffer_offset, "PMEM", 4) == 0) {
object->pmem20.log.current.current = object->pmem20.log.current.next;
buffer_offset += 4;
object->pmem20.log.current.next = read32inc(object->pmem20.log.buffer, &buffer_offset);
object->pmem20.log.current.prev = read32inc(object->pmem20.log.buffer, &buffer_offset);
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
buffer_offset += tmp_len;
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
if (libambit_pmem20_log_parse_header(object->pmem20.log.buffer + buffer_offset, tmp_len, log_header) == 0) {
LOG_INFO("Log entry header parsed");
ret = 1;
}
else {
LOG_ERROR("Failed to parse log entry header correctly");
}
}
else {
LOG_ERROR("Failed to find valid log entry header start");
}
}
else {
LOG_WARNING("Failed to read log entry header");
}
// Unset initialized of something went wrong
if (ret < 0) {
object->pmem20.log.initialized = false;
}
return ret;
}
ambit_log_entry_t *libambit_pmem20_log_read_entry(ambit_object_t *object)
{
// Note! We assume that the caller has called libambit_pmem20_log_next_header just before
uint8_t *periodic_sample_spec;
uint16_t tmp_len, sample_len;
size_t buffer_offset, sample_count = 0, i;
ambit_log_entry_t *log_entry;
ambit_log_sample_t *last_periodic = NULL, *utcsource = NULL, *altisource = NULL;
ambit_date_time_t utcbase;
uint32_t altisource_index = 0;
uint32_t last_base_lat = 0, last_base_long = 0;
uint32_t last_small_lat = 0, last_small_long = 0;
uint32_t last_ehpe = 0;
if (!object->pmem20.log.initialized) {
LOG_ERROR("Trying to get log entry without initialization");
return NULL;
}
// Allocate log entry
if ((log_entry = calloc(1, sizeof(ambit_log_entry_t))) == NULL) {
object->pmem20.log.initialized = false;
return NULL;
}
LOG_INFO("Reading log entry from address=%08x", object->pmem20.log.current.current);
buffer_offset = (object->pmem20.log.current.current - PMEM20_LOG_START);
buffer_offset += 12;
// Read samples content definition
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
periodic_sample_spec = object->pmem20.log.buffer + buffer_offset;
buffer_offset += tmp_len;
// Parse header
tmp_len = read16inc(object->pmem20.log.buffer, &buffer_offset);
if (libambit_pmem20_log_parse_header(object->pmem20.log.buffer + buffer_offset, tmp_len, &log_entry->header) != 0) {
LOG_ERROR("Failed to parse log entry header correctly");
free(log_entry);
object->pmem20.log.initialized = false;
return NULL;
}
buffer_offset += tmp_len;
// Now that we know number of samples, allocate space for them!
if ((log_entry->samples = calloc(log_entry->header.samples_count, sizeof(ambit_log_sample_t))) == NULL) {
free(log_entry);
object->pmem20.log.initialized = false;
return NULL;
}
log_entry->samples_count = log_entry->header.samples_count;
LOG_INFO("Log entry got %d samples, reading", log_entry->samples_count);
// OK, so we are at start of samples, get them all!
while (sample_count < log_entry->samples_count) {
/* NOTE! The double reads below seems a bit unoptimized,
but if we need optimization, we should optimize read_upto
instead...
To ease the pain on wraparound we simply duplicate the sample
to the end of the buffer. */
// First check for log area wrap
if (buffer_offset >= PMEM20_LOG_SIZE - 1) {
read_upto(object, PMEM20_LOG_START + PMEM20_LOG_WRAP_START_OFFSET, 2);
sample_len = read16(object->pmem20.log.buffer, PMEM20_LOG_WRAP_START_OFFSET);
}
else if (buffer_offset == PMEM20_LOG_SIZE - 2) {
read_upto(object, PMEM20_LOG_START + PMEM20_LOG_WRAP_START_OFFSET, 1);
sample_len = object->pmem20.log.buffer[buffer_offset] | (object->pmem20.log.buffer[PMEM20_LOG_WRAP_START_OFFSET] << 8);
}
else {
read_upto(object, PMEM20_LOG_START + buffer_offset, 2);
sample_len = read16(object->pmem20.log.buffer, buffer_offset);
}
// Read all data
if (buffer_offset + 2 < (PMEM20_LOG_SIZE-1)) {
read_upto(object, PMEM20_LOG_START + buffer_offset + 2, sample_len);
}
if (buffer_offset + 2 + sample_len > PMEM20_LOG_SIZE) {
read_upto(object, PMEM20_LOG_START + PMEM20_LOG_WRAP_START_OFFSET, (buffer_offset + 2 + sample_len) - PMEM20_LOG_SIZE);
memcpy(object->pmem20.log.buffer + PMEM20_LOG_SIZE, object->pmem20.log.buffer + PMEM20_LOG_WRAP_START_OFFSET, (buffer_offset + 2 + sample_len) - PMEM20_LOG_SIZE);
}
if (parse_sample(object->pmem20.log.buffer, buffer_offset, &periodic_sample_spec, log_entry, &sample_count) == 1) {
// Calculate times
if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_periodic) {
last_periodic = &log_entry->samples[sample_count-1];
}
else if (last_periodic != NULL) {
log_entry->samples[sample_count-1].time += last_periodic->time;
}
else {
log_entry->samples[sample_count-1].time = 0;
}
if (utcsource == NULL && log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_base) {
utcsource = &log_entry->samples[sample_count-1];
// Calculate UTC base time
add_time(&utcsource->u.gps_base.utc_base_time, 0-utcsource->time, &utcbase);
}
// Calculate positions
if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_base) {
last_base_lat = log_entry->samples[sample_count-1].u.gps_base.latitude;
last_base_long = log_entry->samples[sample_count-1].u.gps_base.longitude;
last_small_lat = log_entry->samples[sample_count-1].u.gps_base.latitude;
last_small_long = log_entry->samples[sample_count-1].u.gps_base.longitude;
last_ehpe = log_entry->samples[sample_count-1].u.gps_base.ehpe;
}
else if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_small) {
log_entry->samples[sample_count-1].u.gps_small.latitude = last_base_lat + log_entry->samples[sample_count-1].u.gps_small.latitude*10;
log_entry->samples[sample_count-1].u.gps_small.longitude = last_base_long + log_entry->samples[sample_count-1].u.gps_small.longitude*10;
last_small_lat = log_entry->samples[sample_count-1].u.gps_small.latitude;
last_small_long = log_entry->samples[sample_count-1].u.gps_small.longitude;
last_ehpe = log_entry->samples[sample_count-1].u.gps_small.ehpe;
}
else if (log_entry->samples[sample_count-1].type == ambit_log_sample_type_gps_tiny) {
log_entry->samples[sample_count-1].u.gps_tiny.latitude = last_small_lat + log_entry->samples[sample_count-1].u.gps_tiny.latitude*10;
log_entry->samples[sample_count-1].u.gps_tiny.longitude = last_small_long + log_entry->samples[sample_count-1].u.gps_tiny.longitude*10;
log_entry->samples[sample_count-1].u.gps_tiny.ehpe = (last_ehpe > 700 ? 700 : last_ehpe);
last_small_lat = log_entry->samples[sample_count-1].u.gps_tiny.latitude;
last_small_long = log_entry->samples[sample_count-1].u.gps_tiny.longitude;
}
if (altisource == NULL && log_entry->samples[sample_count-1].type == ambit_log_sample_type_altitude_source) {
altisource = &log_entry->samples[sample_count-1];
altisource_index = sample_count-1;
}
}
buffer_offset += 2 + sample_len;
// Wrap
if (buffer_offset >= PMEM20_LOG_SIZE) {
buffer_offset = PMEM20_LOG_WRAP_START_OFFSET + (buffer_offset - PMEM20_LOG_SIZE);
}
}
// Loop through samples again and correct times etc
for (sample_count = 0; sample_count < log_entry->header.samples_count; sample_count++) {
// Set UTC times (if UTC source found)
if (utcsource != NULL) {
add_time(&utcbase, log_entry->samples[sample_count].time, &log_entry->samples[sample_count].utc_time);
}
// Correct altitude based on altitude offset in altitude source
if (altisource != NULL && log_entry->samples[sample_count].type == ambit_log_sample_type_periodic && sample_count < altisource_index) {
for (i=0; i<log_entry->samples[sample_count].u.periodic.value_count; i++) {
if (log_entry->samples[sample_count].u.periodic.values[i].type == ambit_log_sample_periodic_type_sealevelpressure) {
log_entry->samples[sample_count].u.periodic.values[i].u.sealevelpressure += altisource->u.altitude_source.pressure_offset;
}
if (log_entry->samples[sample_count].u.periodic.values[i].type == ambit_log_sample_periodic_type_altitude) {
log_entry->samples[sample_count].u.periodic.values[i].u.altitude += altisource->u.altitude_source.altitude_offset;
}
}
}
}
return log_entry;
}
static int log_read(ambit_object_t *object, ambit_log_skip_cb skip_cb, ambit_log_push_cb push_cb, ambit_log_progress_cb progress_cb, void *userref)
{
int entries_read = 0;
uint8_t *reply_data = NULL;
size_t replylen = 0;
uint16_t log_entries_total = 0;
uint16_t log_entries_walked = 0;
uint32_t more = 0x00000400;
bool read_pmem = false;
ambit_log_header_t log_header;
ambit_log_entry_t *log_entry;
LOG_INFO("Reading number of logs");
log_header.activity_name = NULL;
/*
* Read number of log entries
*/
if (libambit_protocol_command(object, ambit_command_log_count, NULL, 0, &reply_data, &replylen, 0) != 0) {
LOG_WARNING("Failed to read number of log entries");
return -1;
}
log_entries_total = le16toh(*(uint16_t*)(reply_data + 2));
libambit_protocol_free(reply_data);
LOG_INFO("Number of logs=%d", log_entries_total);
/*
* First part walks through headers to check if there is any point in start
* reading the PMEM content. If no skip callback is defined, there is no
* point in checking the headers, because no one can tell us to not include
* the logs...
*/
if (skip_cb != NULL) {
LOG_INFO("Look in headers for new logs");
// Rewind
if (libambit_protocol_command(object, ambit_command_log_head_first, NULL, 0, &reply_data, &replylen, 0) != 0) {
LOG_WARNING("Failed to rewind header pointer");
return -1;
}
more = le32toh(*(uint32_t*)reply_data);
libambit_protocol_free(reply_data);
// Loop through logs while more entries exists
while (more == 0x00000400) {
LOG_INFO("Reading next header");
// Go to next entry
if (libambit_protocol_command(object, ambit_command_log_head_step, NULL, 0, &reply_data, &replylen, 0) != 0) {
LOG_WARNING("Failed to walk to next header");
return -1;
}
libambit_protocol_free(reply_data);
// Assume every header is composited by 2 parts, where only the
// second is of interrest right now
if (libambit_protocol_command(object, ambit_command_log_head, NULL, 0, &reply_data, &replylen, 0) != 0) {
LOG_WARNING("Failed to read first part of header");
return -1;
}
libambit_protocol_free(reply_data);
if (libambit_protocol_command(object, ambit_command_log_head, NULL, 0, &reply_data, &replylen, 0) == 0) {
if (replylen > 8 && libambit_pmem20_log_parse_header(reply_data + 8, replylen - 8, &log_header) == 0) {
if (skip_cb(userref, &log_header) != 0) {
// Header was NOT skipped, break out!
read_pmem = true;
LOG_INFO("Found new entry, start reading log data");
break;
}
}
else {
LOG_ERROR("Failed to parse log header");
return -1;
}
libambit_protocol_free(reply_data);
}
else {
LOG_WARNING("Failed to read second part of header");
return -1;
}
// Is there more entries to read?
if (libambit_protocol_command(object, ambit_command_log_head_peek, NULL, 0, &reply_data, &replylen, 0) != 0) {
LOG_WARNING("Failed to check for more headers");
return -1;
}
more = le32toh(*(uint32_t*)reply_data);
libambit_protocol_free(reply_data);
}
}
else {
LOG_INFO("No skip callback defined, reading log data");
read_pmem = true;
}
if (read_pmem) {
if (libambit_pmem20_log_init(&object->driver_data->pmem20, PMEM20_LOG_START, PMEM20_LOG_SIZE) != 0) {
return -1;
}
// Loop through all log entries, first check headers
while (log_entries_walked < log_entries_total && libambit_pmem20_log_next_header(&object->driver_data->pmem20, &log_header) == 1) {
LOG_INFO("Reading header of log %d of %d", log_entries_walked + 1, log_entries_total);
if (progress_cb != NULL) {
progress_cb(userref, log_entries_total, log_entries_walked+1, 100*log_entries_walked/log_entries_total);
}
// Check if this entry needs to be read
if (skip_cb == NULL || skip_cb(userref, &log_header) != 0) {
LOG_INFO("Reading data of log %d of %d", log_entries_walked + 1, log_entries_total);
log_entry = libambit_pmem20_log_read_entry(&object->driver_data->pmem20);
if (log_entry != NULL) {
if (push_cb != NULL) {
push_cb(userref, log_entry);
}
entries_read++;
}
}
else {
LOG_INFO("Log %d of %d already exists, skip reading data", log_entries_walked + 1, log_entries_total);
}
log_entries_walked++;
if (progress_cb != NULL) {
progress_cb(userref, log_entries_total, log_entries_walked, 100*log_entries_walked/log_entries_total);
}
}
}
LOG_INFO("%d entries read", entries_read);
return entries_read;
}
