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; }