int libambit_sbem0102_command_request_raw(libambit_sbem0102_t *object, uint16_t command, uint8_t *data, size_t datalen, libambit_sbem0102_data_t *reply_data)
{
int ret = -1;
uint8_t *reply = NULL;
size_t replylen = 0;
static uint8_t header[] = { 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 'S', 'B', 'E', 'M', '0', '1', '0', '2' };
// Reset reply data before starting to fill it
libambit_sbem0102_data_free(reply_data);
if (libambit_protocol_command(object->ambit_object, command, data, datalen, &reply, &replylen, 0) == 0) {
// Check that the reply contains an SBEM0102 header
if (replylen >= sizeof(header) && memcmp(reply + 6, header + 6, 8) == 0) {
if (replylen > sizeof(header)) {
// Copy message to reply_data object
reply_data->data = malloc(replylen - sizeof(header));
memcpy(reply_data->data, reply + sizeof(header), replylen - sizeof(header));
reply_data->size = replylen - sizeof(header);
// LOG_INFO("Command 0x%x Copy message to reply_data object size = %d",command,reply_data->size);
}
ret = 0;
}
libambit_protocol_free(reply);
}
return ret;
}
static int get_memory_maps(ambit_object_t *object)
{
uint8_t *reply_data = NULL;
size_t replylen = 0;
uint8_t send_data[4] = { 0x00, 0x00, 0x00, 0x00 };
libambit_sbem0102_data_t reply_data_object;
memory_map_entry_t *mm_entry;
const uint8_t *ptr;
if (libambit_protocol_command(object, ambit_command_unknown2, NULL, 0, &reply_data, &replylen, 0) != 0 || replylen < 4) {
libambit_protocol_free(reply_data);
LOG_WARNING("Failed to read memory map key");
return -1;
}
libambit_protocol_free(reply_data);
libambit_sbem0102_data_init(&reply_data_object);
if (libambit_sbem0102_command_request_raw(&object->driver_data->sbem0102, ambit_command_ambit3_memory_map, send_data, sizeof(send_data), &reply_data_object) != 0) {
LOG_WARNING("Failed to read memory map");
return -1;
}
while (libambit_sbem0102_data_next(&reply_data_object) == 0) {
if (libambit_sbem0102_data_id(&reply_data_object) == 0x3f) {
ptr = libambit_sbem0102_data_ptr(&reply_data_object);
mm_entry = NULL;
if (strcmp((char*)ptr, "Waypoints") == 0) {
mm_entry = &object->driver_data->memory_maps.waypoints;
}
else if (strcmp((char*)ptr, "Routes") == 0) {
mm_entry = &object->driver_data->memory_maps.waypoints;
}
else if (strcmp((char*)ptr, "Rules") == 0) {
mm_entry = &object->driver_data->memory_maps.rules;
}
else if (strcmp((char*)ptr, "GpsSGEE") == 0) {
mm_entry = &object->driver_data->memory_maps.gps;
}
else if (strcmp((char*)ptr, "CustomModes") == 0) {
mm_entry = &object->driver_data->memory_maps.custom_modes;
}
else if (strcmp((char*)ptr, "TrainingProgram") == 0) {
mm_entry = &object->driver_data->memory_maps.training_program;
}
else if (strcmp((char*)ptr, "ExerciseLog") == 0) {
mm_entry = &object->driver_data->memory_maps.excercise_log;
}
else if (strcmp((char*)ptr, "EventLog") == 0) {
mm_entry = &object->driver_data->memory_maps.event_log;
}
else if (strcmp((char*)ptr, "BlePairingInfo") == 0) {
mm_entry = &object->driver_data->memory_maps.ble_pairing;
}
else {
LOG_WARNING("Unknown memory map type \"%s\"", (char*)ptr);
}
if (mm_entry != NULL) {
// We have dealed with the name, advance to hash
ptr += strlen((char*)ptr) + 1;
if (libambit_htob((const char*)ptr, mm_entry->hash, sizeof(mm_entry->hash)) < 0) {
LOG_ERROR("Failed to read memory map hash");
}
ptr += strlen((char*)ptr) + 1;
mm_entry->start = read32(ptr, 0);
ptr += 4;
mm_entry->size = read32(ptr, 0);
}
}
}
object->driver_data->memory_maps.initialized = 1;
libambit_sbem0102_data_free(&reply_data_object);
LOG_INFO("Memory map successfully parsed");
return 0;
}
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;
uint16_t log_entries_total = 0;
uint16_t log_entries_walked = 0;
uint16_t log_entries_notsynced = 0;
ambit3_log_header_t log_header;
ambit_log_entry_t *log_entry;
libambit_sbem0102_data_t send_data_object, reply_data_object;
LOG_INFO("Reading log headers");
log_header.header.activity_name = NULL;
libambit_sbem0102_data_init(&send_data_object);
libambit_sbem0102_data_init(&reply_data_object);
libambit_sbem0102_data_add(&send_data_object, 0x8d, NULL, 0);
if (libambit_sbem0102_command_request(&object->driver_data->sbem0102, ambit_command_ambit3_log_headers, &send_data_object, &reply_data_object) != 0) {
LOG_WARNING("Failed to read log headers");
return -1;
}
if (object->driver_data->memory_maps.initialized == 0) {
if (get_memory_maps(object) != 0) {
return -1;
}
}
// Initialize PMEM20 log before starting to read logs
libambit_pmem20_log_init(&object->driver_data->pmem20, object->driver_data->memory_maps.excercise_log.start, object->driver_data->memory_maps.excercise_log.size);
while (libambit_sbem0102_data_next(&reply_data_object) == 0) {
switch (libambit_sbem0102_data_id(&reply_data_object)) {
case 0x4e:
log_entries_total = read16(libambit_sbem0102_data_ptr(&reply_data_object), 0);
LOG_INFO("Number of logs=%d", log_entries_total);
break;
case 0x4f:
log_entries_notsynced = read16(libambit_sbem0102_data_ptr(&reply_data_object), 0);
LOG_INFO("Number of logs marked as not syncronized=%d", log_entries_notsynced);
break;
case 0x7e:
if (parse_log_header(libambit_sbem0102_data_ptr(&reply_data_object), &log_header) == 0) {
LOG_INFO("Log header parsed successfully");
if (skip_cb(userref, &log_header.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_address(&object->driver_data->pmem20, log_header.address, log_header.end_address - log_header.address);
if (log_entry != NULL) {
if (push_cb != NULL) {
push_cb(userref, log_entry);
}
entries_read++;
}
}
else {
LOG_INFO("Log entry already exists, skipping");
}
}
else {
LOG_INFO("Failed to parse log header");
}
log_entries_walked++;
if (progress_cb != NULL) {
progress_cb(userref, log_entries_total, log_entries_walked, 100*log_entries_walked/log_entries_total);
}
break;
default:
break;
}
}
libambit_sbem0102_data_free(&send_data_object);
libambit_sbem0102_data_free(&reply_data_object);
return entries_read;
}
int libambit_sbem0102_command_request(libambit_sbem0102_t *object, uint16_t command, libambit_sbem0102_data_t *data_objects, libambit_sbem0102_data_t *reply_data)
{
int ret = -1;
uint8_t *send_data = NULL;
size_t offset = 0;
uint8_t *reply = NULL;
size_t replylen = 0;
static uint8_t header[] = { 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 'S', 'B', 'E', 'M', '0', '1', '0', '2' };
static uint8_t special_header[] = { 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x0a, 0x00, 'S', 'B', 'E', 'M', '0', '1', '0', '2' };
// TODO: We have no idea how to deal with multiple packets at the moment,
// just fail for now
if (data_objects != NULL && data_objects->size > object->chunk_size) {
LOG_INFO("libambit_sbem0102_command_request - --1");
return -1;
}
// Calculate size of buffer, and allocate it
// TODO: log headers seems to have a different format than the rest, treat
// it here until the mystery of the 2 extra bytes is really solved
if (command == ambit_command_ambit3_log_headers) {
send_data = malloc(sizeof(special_header) + (data_objects != NULL ? data_objects->size : 0));
if (send_data == NULL) {
return -1;
}
memcpy(send_data, special_header, sizeof(special_header));
offset += sizeof(special_header);
}
else {
send_data = malloc(sizeof(header) + (data_objects != NULL ? data_objects->size : 0));
if (send_data == NULL) {
return -1;
}
memcpy(send_data, header, sizeof(header));
offset += sizeof(header);
}
// Add data objects
if (data_objects != NULL && data_objects->data != NULL && data_objects->size > 0) {
memcpy(send_data+offset, data_objects->data, data_objects->size);
offset += data_objects->size;
}
// Reset reply data before starting to fill it
libambit_sbem0102_data_free(reply_data);
if (libambit_protocol_command(object->ambit_object, command, send_data, offset, &reply, &replylen, 0) == 0) {
// Check that the reply contains an SBEM0102 header
if (replylen >= sizeof(header) && memcmp(reply + 6, header + 6, 8) == 0) {
if (replylen > sizeof(header)) {
// Copy message to reply_data object
reply_data->data = malloc(replylen - sizeof(header));
memcpy(reply_data->data, reply + sizeof(header), replylen - sizeof(header));
reply_data->size = replylen - sizeof(header);
// Check if this reply was just a part (5th byte is the current
// guess on how to determine)
while (reply[4] != 0x01) {
// Guess number 2: first 4 bytes seems to be copied from
// the reply when asking for more data, what the f*ck does
// they represent!?
memcpy(send_data, reply, 4);
// Free old reply before calling again
libambit_protocol_free(reply);
if (libambit_protocol_command(object->ambit_object, command, send_data, offset, &reply, &replylen, 0) != 0 ||
replylen < 6) {
libambit_sbem0102_data_free(reply_data);
break;
}
if (replylen > 6) {
reply_data->data = realloc(reply_data->data, reply_data->size + replylen - 6);
memcpy(reply_data->data + reply_data->size, reply + 6, replylen - 6);
reply_data->size += replylen - 6;
}
}
}
ret = 0;
}
libambit_protocol_free(reply);
LOG_INFO("%2x %2x %2x %2x %2x %2x %2x %2x",reply_data->data[0],reply_data->data[1],reply_data->data[2],reply_data->data[3],reply_data->data[4],reply_data->data[5],reply_data->data[6],reply_data->data[7]);
}
free(send_data);
return ret;
}
