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 gps_orbit_write(ambit_object_t *object, uint8_t *data, size_t datalen)
{
uint8_t header[8], cmpheader[8];
int ret = -1;
LOG_INFO("Writing GPS orbit data");
libambit_protocol_command(object, ambit_command_write_start, NULL, 0, NULL, NULL, 0);
if (object->driver->gps_orbit_header_read(object, header) == 0) {
cmpheader[0] = data[7]; // Year, swap bytes
cmpheader[1] = data[6];
cmpheader[2] = data[8];
cmpheader[3] = data[9];
cmpheader[4] = data[13]; // 4 byte swap
cmpheader[5] = data[12];
cmpheader[6] = data[11];
cmpheader[7] = data[10];
// Check if new data differs
if (memcmp(header, cmpheader, 8) != 0) {
ret = libambit_pmem20_gps_orbit_write(&object->driver_data->pmem20, data, datalen, true);
}
else {
LOG_INFO("Current GPS orbit data is already up to date, skipping");
ret = 0;
}
}
return ret;
}
static int device_info_get(ambit_object_t *object, ambit_device_info_t *info)
{
uint8_t *reply_data = NULL;
size_t replylen;
int ret = -1;
LOG_INFO("Reading device info");
if (libambit_protocol_command(object, ambit_command_device_info, komposti_version, sizeof(komposti_version), &reply_data, &replylen, 1) == 0) {
if (info != NULL) {
const char *p = (char *)reply_data;
info->model = utf8memconv(p, LIBAMBIT_MODEL_LENGTH, NULL);
p += LIBAMBIT_MODEL_LENGTH;
info->serial = utf8memconv(p, LIBAMBIT_SERIAL_LENGTH, NULL);
p += LIBAMBIT_SERIAL_LENGTH;
memcpy(info->fw_version, p, 4);
memcpy(info->hw_version, p + 4, 4);
}
ret = 0;
}
else {
LOG_WARNING("Failed to device info");
}
libambit_protocol_free(reply_data);
return ret;
}
static int read_log_chunk(ambit_object_t *object, uint32_t address)
{
int ret = -1;
uint8_t *buffer = object->pmem20.log.buffer + (address - PMEM20_LOG_START);
uint32_t length = object->pmem20.chunk_size;
uint8_t *reply = NULL;
size_t replylen = 0;
uint8_t send_data[8];
uint32_t *_address = (uint32_t*)&send_data[0];
uint32_t *_length = (uint32_t*)&send_data[4];
if ((address + object->pmem20.chunk_size) > (PMEM20_LOG_START + PMEM20_LOG_SIZE)) {
length = PMEM20_LOG_START + PMEM20_LOG_SIZE - address;
}
*_address = htole32(address);
*_length = htole32(length);
if (libambit_protocol_command(object, ambit_command_log_read, send_data, sizeof(send_data), &reply, &replylen, 0) == 0 &&
replylen == length + 8) {
memcpy(buffer, reply + 8, length);
ret = 0;
}
libambit_protocol_free(reply);
return ret;
}
static int gps_orbit_header_read(ambit_object_t *object, uint8_t data[8])
{
uint8_t *reply_data = NULL;
size_t replylen = 0;
int ret = -1;
if (libambit_protocol_command(object, ambit_command_gps_orbit_head, NULL, 0, &reply_data, &replylen, 0) == 0 && replylen >= 9) {
memcpy(data, &reply_data[1], 8);
libambit_protocol_free(reply_data);
ret = 0;
}
else {
LOG_WARNING("Failed to read GPS orbit header");
}
return ret;
}
static int personal_settings_get(ambit_object_t *object, ambit_personal_settings_t *settings)
{
uint8_t *reply_data = NULL;
size_t replylen = 0;
int ret = -1;
LOG_INFO("Reading personal settings");
if (libambit_protocol_command(object, ambit_command_personal_settings, NULL, 0, &reply_data, &replylen, 0) == 0) {
ret = libambit_personal_settings_parse(reply_data, replylen, settings);
libambit_protocol_free(reply_data);
}
else {
LOG_WARNING("Failed to read personal settings");
}
return ret;
}
int libambit_pmem20_gps_orbit_write(ambit_object_t *object, uint8_t *data, size_t datalen)
{
int ret = -1;
uint8_t *bufptrs[2];
size_t bufsizes[2];
uint8_t startheader[4];
uint8_t tailbuf[8];
uint32_t *_sizeptr = (uint32_t*)&startheader[0];
uint32_t address = PMEM20_GPS_ORBIT_START;
size_t offset = 0;
*_sizeptr = htole32(datalen);
bufptrs[0] = startheader;
bufsizes[0] = 4;
bufptrs[1] = data;
bufsizes[1] = object->pmem20.chunk_size - 4; // We assume that data is
// always > chunk_size
// Write first chunk (including length)
ret = write_data_chunk(object, address, 2, bufptrs, bufsizes);
offset += bufsizes[1];
address += object->pmem20.chunk_size;
// Write rest of the chunks
while (ret == 0 && offset < datalen) {
bufptrs[0] = data + offset;
bufsizes[0] = (datalen - offset > object->pmem20.chunk_size ? object->pmem20.chunk_size : datalen - offset);
ret = write_data_chunk(object, address, 1, bufptrs, bufsizes);
offset += bufsizes[0];
address += bufsizes[0];
}
// Write tail length (or what is really!?)
if (ret == 0) {
*((uint32_t*)(&tailbuf[0])) = htole32(PMEM20_GPS_ORBIT_START);
*((uint32_t*)(&tailbuf[4])) = htole32(bufsizes[0]);
ret = libambit_protocol_command(object, ambit_command_data_tail_len, tailbuf, sizeof(tailbuf), NULL, NULL, 0);
}
return ret;
}
static int write_data_chunk(ambit_object_t *object, uint32_t address, size_t buffer_count, uint8_t **buffers, size_t *buffer_sizes)
{
int ret = -1;
uint8_t *reply = NULL;
size_t replylen = 0;
uint8_t *send_data;
size_t send_data_len = 8;
int i;
for (i=0; i<buffer_count; i++) {
send_data_len += buffer_sizes[i];
}
send_data = malloc(send_data_len);
if (send_data != NULL) {
uint32_t *_address = (uint32_t*)&send_data[0];
uint32_t *_length = (uint32_t*)&send_data[4];
uint8_t *send_data_ptr = &send_data[8];
*_address = htole32(address);
*_length = htole32(send_data_len - 8);
for (i=0; i<buffer_count; i++) {
memcpy(send_data_ptr, buffers[i], buffer_sizes[i]);
send_data_ptr += buffer_sizes[i];
}
if (libambit_protocol_command(object, ambit_command_data_write, send_data, send_data_len, &reply, &replylen, 0) == 0) {
ret = 0;
}
free(send_data);
libambit_protocol_free(reply);
}
return ret;
}
int libambit_sbem0102_write(libambit_sbem0102_t *object, uint16_t command, libambit_sbem0102_data_t *data)
{
int ret = -1;
uint8_t *send_data;
size_t offset = 0;
uint8_t *reply = NULL;
size_t replylen = 0;
static uint8_t header[] = { 0x00, 0x00, 0x00, 0x00, 0x01, 0x01, '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 != NULL && data->size > object->chunk_size) {
return -1;
}
// Calculate size of buffer, and allocate it
send_data = malloc(sizeof(header) + (data != NULL ? data->size : 0));
if (send_data == NULL) {
return -1;
}
// Prepare initial header
memcpy(send_data, header, sizeof(header));
offset += sizeof(header);
if (data != NULL && data->data != NULL && data->size > 0) {
memcpy(send_data+offset, data->data, data->size);
offset += data->size;
}
ret = libambit_protocol_command(object->ambit_object, command, send_data, offset, &reply, &replylen, 0);
free(send_data);
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;
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;
}
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;
}
