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