Lurkars
/
esp-ena
mirror of https://github.com/Lurkars/esp-ena.git
1
0
Fork 0
Code Issues Projects Releases Wiki Activity
esp-ena/main/ena-storage.c

449 lines
17 KiB
C
Raw Blame History

#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "esp_partition.h"
#include "esp_spi_flash.h"
#include "esp_log.h"
#include "ena-storage.h"
#include "ena-crypto.h"
#define BLOCK_SIZE 4096
void ena_storage_read(size_t address, uint8_t *data, size_t size)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_read");
const esp_partition_t *partition = esp_partition_find_first(
ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, PARTITION_NAME);
assert(partition);
ESP_ERROR_CHECK(esp_partition_read(partition, address, data, size));
vTaskDelay(1);
ESP_LOGD(ENA_STORAGE_LOG, "read data at %u", address);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, data, size, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_read");
}
void ena_storage_write(size_t address, uint8_t data[], size_t size)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_write");
const esp_partition_t *partition = esp_partition_find_first(
ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, PARTITION_NAME);
assert(partition);
const int block_num = address / BLOCK_SIZE;
// split if size extends block
if (address + size > (block_num + 1) * BLOCK_SIZE)
{
ESP_LOGI(ENA_STORAGE_LOG, "overflow block at address %u with size %d (block %d)", address, size, block_num);
const size_t block1_address = address;
const size_t block2_address = (block_num + 1) * BLOCK_SIZE;
const size_t data2_size = address + size - block2_address;
const size_t data1_size = size - data2_size;
ESP_LOGD(ENA_STORAGE_LOG, "block1_address %d, block1_size %d (block %d)", block1_address, data1_size, block_num);
ESP_LOGD(ENA_STORAGE_LOG, "block2_address %d, block2_size %d (block %d)", block2_address, data2_size, block_num + 1);
uint8_t *data1 = malloc(data1_size);
for (int i = 0; i < data1_size; i++)
{
data1[i] = data[i];
}
uint8_t *data2 = malloc(data2_size);
for (int i = 0; i < data2_size; i++)
{
data2[i] = data[data1_size + i];
}
ena_storage_write(block1_address, data1, data1_size);
free(data1);
ena_storage_write(block2_address, data2, data2_size);
free(data2);
}
else
{
const int block_start = block_num * BLOCK_SIZE;
const int block_address = address - block_start;
uint8_t *buffer = malloc(BLOCK_SIZE);
if (buffer == NULL)
{
ESP_LOGE(ENA_STORAGE_LOG, "Warning %s malloc low memory", "buffer");
return;
}
ESP_LOGD(ENA_STORAGE_LOG, "read block %d buffer: start %d size %u", block_num, block_start, BLOCK_SIZE);
ESP_ERROR_CHECK(esp_partition_read(partition, block_start, buffer, BLOCK_SIZE));
vTaskDelay(1);
ESP_ERROR_CHECK(esp_partition_erase_range(partition, block_start, BLOCK_SIZE));
for (int i = 0; i < size; i++)
{
buffer[block_address + i] = data[i];
}
ESP_ERROR_CHECK(esp_partition_write(partition, block_start, buffer, BLOCK_SIZE));
free(buffer);
ESP_LOGD(ENA_STORAGE_LOG, "write data at %u", address);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, data, size, ESP_LOG_DEBUG);
}
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_write");
}
void ena_storage_shift_delete(size_t address, size_t size)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_shift_delete");
const esp_partition_t *partition = esp_partition_find_first(
ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, PARTITION_NAME);
assert(partition);
const int block_num = address / BLOCK_SIZE;
// split if size extends block TODO this does not work like this!!! Shift puts data back to first block!
if (address + size > (block_num + 1) * BLOCK_SIZE)
{
const int block1_address = address;
const int block2_address = (block_num + 1) * BLOCK_SIZE;
const int data2_size = address + size - block2_address;
const int data1_size = size - data2_size;
ena_storage_shift_delete(block1_address, data1_size);
ena_storage_shift_delete(block2_address, data2_size);
}
else
{
const int block_start = block_num * BLOCK_SIZE;
const int block_address = address - block_start;
uint8_t *buffer = malloc(BLOCK_SIZE);
ESP_ERROR_CHECK(esp_partition_read(partition, block_start, buffer, BLOCK_SIZE));
vTaskDelay(1);
ESP_LOGD(ENA_STORAGE_LOG, "shift block from %u to %u with size %u (move %u)", block_address, block_address + size, size, BLOCK_SIZE - address + size);
// shift manually
for (int i = block_address + size; i < BLOCK_SIZE; i++)
{
buffer[i - size] = buffer[i];
}
for (int i = BLOCK_SIZE - size; i < BLOCK_SIZE; i++)
{
buffer[i] = 0;
}
// memmove seems to lead to corrupt heap memory!
// memmove(&buffer[block_address], &buffer[block_address + size], BLOCK_SIZE - address + size);
ESP_ERROR_CHECK(esp_partition_erase_range(partition, block_start, BLOCK_SIZE));
ESP_ERROR_CHECK(esp_partition_write(partition, block_start, buffer, BLOCK_SIZE));
free(buffer);
}
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_shift_delete");
}
void ena_storage_write_tek(uint32_t enin, uint8_t tek[])
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_write_tek");
uint8_t tek_count = ena_storage_read_u8(ENA_STORAGE_TEK_COUNT_ADDRESS);
size_t address = ENA_STORAGE_TEK_START_ADDRESS + (tek_count * ENA_STORAGE_TEK_LENGTH);
ena_storage_write_u32(address, enin);
ena_storage_write(address + 4, tek, ENA_KEY_LENGTH);
tek_count++;
if (tek_count > ENA_STOARGE_TEK_STORE_PERIOD)
{
tek_count = 0;
}
ena_storage_write_u8(ENA_STORAGE_TEK_COUNT_ADDRESS, tek_count);
ESP_LOGD(ENA_STORAGE_LOG, "write tek: ENIN %u", enin);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, tek, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_write_tek");
}
uint32_t ena_storage_read_enin(void)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_read_enin");
uint8_t tek_count = ena_storage_read_u8(ENA_STORAGE_TEK_COUNT_ADDRESS);
if (tek_count < 1)
{
return 0;
}
size_t address = ENA_STORAGE_TEK_START_ADDRESS + (tek_count - 1) * ENA_STORAGE_TEK_LENGTH;
uint32_t result = ena_storage_read_u32(address);
ESP_LOGD(ENA_STORAGE_LOG, "read last ENIN: %u", result);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_read_enin");
return result;
}
void ena_storage_read_tek(uint8_t tek[])
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_read_tek");
uint8_t tek_count = ena_storage_read_u8(ENA_STORAGE_TEK_COUNT_ADDRESS);
if (tek_count < 1)
{
return;
}
size_t address = ENA_STORAGE_TEK_START_ADDRESS + (tek_count - 1) * ENA_STORAGE_TEK_LENGTH + 4;
ena_storage_read(address, tek, ENA_KEY_LENGTH);
ESP_LOGD(ENA_STORAGE_LOG, "read last tek:");
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, tek, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_read_tek");
}
uint32_t ena_storage_temp_detections_count(void)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_temp_detections_count");
uint32_t count = ena_storage_read_u32(ENA_STORAGE_TEMP_DETECTIONS_COUNT_ADDRESS);
ESP_LOGD(ENA_STORAGE_LOG, "read temp contancts count: %u", count);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_temp_detections_count");
return count;
}
uint32_t ena_storage_write_temp_detection(uint32_t timestamp, uint8_t rpi[], uint8_t aem[], int rssi)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_write_temp_detection");
uint32_t count = ena_storage_temp_detections_count() + 1;
// start overwriting temporay detections?!
if (count > ENA_STOARGE_TEMP_DETECTIONS_MAX)
{
count = 1;
}
size_t address = ENA_STORAGE_TEMP_DETECTIONS_START_ADDRESS + (count - 1) * ENA_STORAGE_DETECTION_LENGTH;
ena_storage_write_u32(address, timestamp);
address += 4;
ena_storage_write(address, rpi, ENA_KEY_LENGTH);
address += ENA_KEY_LENGTH;
ena_storage_write(address, aem, ENA_AEM_METADATA_LENGTH);
address += ENA_AEM_METADATA_LENGTH;
ena_storage_write_int(address, rssi);
ena_storage_write_u32(ENA_STORAGE_TEMP_DETECTIONS_COUNT_ADDRESS, count);
ESP_LOGD(ENA_STORAGE_LOG, "write temp detection: timestamp %u and rssi %d", timestamp, rssi);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, rpi, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, aem, ENA_AEM_METADATA_LENGTH, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_write_temp_detection");
return count - 1;
}
void ena_storage_read_temp_detection(uint32_t index, uint32_t *timestamp, uint8_t rpi[], uint8_t aem[], int *rssi)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_read_temp_detection");
size_t address = ENA_STORAGE_TEMP_DETECTIONS_START_ADDRESS + index * ENA_STORAGE_DETECTION_LENGTH;
*timestamp = ena_storage_read_u32(address);
address += 4;
ena_storage_read(address, rpi, ENA_KEY_LENGTH);
address += ENA_KEY_LENGTH;
ena_storage_read(address, aem, ENA_AEM_METADATA_LENGTH);
address += 4;
*rssi = ena_storage_read_int(address);
ESP_LOGD(ENA_STORAGE_LOG, "read temp detection: timestamp %u and rssi %d", *timestamp, *rssi);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, rpi, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, aem, ENA_AEM_METADATA_LENGTH, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_read_temp_detection");
}
void ena_storage_remove_temp_detection(uint32_t index)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_remove_temp_detection");
size_t address = ENA_STORAGE_TEMP_DETECTIONS_START_ADDRESS + index * ENA_STORAGE_DETECTION_LENGTH;
ena_storage_shift_delete(address, ENA_STORAGE_DETECTION_LENGTH);
uint32_t count = ena_storage_temp_detections_count();
count--;
ena_storage_write_u32(ENA_STORAGE_TEMP_DETECTIONS_COUNT_ADDRESS, count);
ESP_LOGD(ENA_STORAGE_LOG, "remove temp detection: %u", index);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_remove_temp_detection");
}
uint32_t ena_storage_detections_count(void)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_detections_count");
uint32_t count = ena_storage_read_u32(ENA_STORAGE_DETECTIONS_COUNT_ADDRESS);
ESP_LOGD(ENA_STORAGE_LOG, "read contancts count: %u", count);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_detections_count");
return count;
}
void ena_storage_write_detection(uint32_t timestamp, uint8_t rpi[], uint8_t aem[], int rssi)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_write_detection");
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, rpi, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
uint32_t count = ena_storage_detections_count() + 1;
size_t address = ENA_STORAGE_DETECTIONS_START_ADDRESS + (count - 1) * ENA_STORAGE_DETECTION_LENGTH;
ena_storage_write_u32(address, timestamp);
address += 4;
ena_storage_write(address, rpi, ENA_KEY_LENGTH);
address += ENA_KEY_LENGTH;
ena_storage_write(address, aem, ENA_AEM_METADATA_LENGTH);
address += ENA_AEM_METADATA_LENGTH;
ena_storage_write_int(address, rssi);
ena_storage_write_u32(ENA_STORAGE_DETECTIONS_COUNT_ADDRESS, count);
ESP_LOGD(ENA_STORAGE_LOG, "write detection: timestamp %u and rssi %d", timestamp, rssi);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, rpi, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, aem, ENA_AEM_METADATA_LENGTH, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_write_detection");
}
void ena_storage_read_detection(uint32_t index, uint32_t *timestamp, uint8_t rpi[], uint8_t aem[], int *rssi)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_read_detection");
size_t address = ENA_STORAGE_DETECTIONS_START_ADDRESS + index * ENA_STORAGE_DETECTION_LENGTH;
*timestamp = ena_storage_read_u32(address);
address += 4;
ena_storage_read(address, rpi, ENA_KEY_LENGTH);
address += ENA_KEY_LENGTH;
ena_storage_read(address, aem, ENA_AEM_METADATA_LENGTH);
address += 4;
*rssi = ena_storage_read_int(address);
ESP_LOGD(ENA_STORAGE_LOG, "read detection: timestamp %u and rssi %d", *timestamp, *rssi);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, rpi, ENA_KEY_LENGTH, ESP_LOG_DEBUG);
ESP_LOG_BUFFER_HEXDUMP(ENA_STORAGE_LOG, aem, ENA_AEM_METADATA_LENGTH, ESP_LOG_DEBUG);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_read_detection");
}
uint8_t ena_storage_read_u8(size_t address)
{
uint8_t data[1] = {0};
ena_storage_read(address, (uint8_t *)&data, 1);
return data[0];
}
uint32_t ena_storage_read_u32(size_t address)
{
uint8_t data[4] = {0};
ena_storage_read(address, (uint8_t *)&data, 4);
uint32_t result = data[0] | (data[1] << 8) | (data[2] << 16) | (data[3] << 24);
return result;
}
void ena_storage_write_u8(size_t address, uint8_t byte)
{
uint8_t data[1] = {byte};
ena_storage_write(address, data, 1);
}
void ena_storage_write_u32(size_t address, uint32_t value)
{
uint8_t *data = malloc(4);
data[0] = (value & 0x000000ff);
data[1] = (value & 0x0000ff00) >> 8;
data[2] = (value & 0x00ff0000) >> 16;
data[3] = (value & 0xff000000) >> 24;
ena_storage_write(address, data, 4);
free(data);
}
int ena_storage_read_int(size_t address)
{
uint8_t data[sizeof(int)] = {0};
ena_storage_read(address, (uint8_t *)&data, sizeof(int));
int result = 0;
memcpy((int *)&result, (int *)&data, sizeof(int));
return result;
}
void ena_storage_write_int(size_t address, int value)
{
uint8_t data[sizeof(int)] = {0};
memcpy((int *)&data, (int *)&value, sizeof(int));
ena_storage_write(address, data, sizeof(int));
}
void ena_storage_erase(void)
{
ESP_LOGD(ENA_STORAGE_LOG, "START ena_storage_erase");
const esp_partition_t *partition = esp_partition_find_first(
ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_ANY, PARTITION_NAME);
assert(partition);
ESP_ERROR_CHECK(esp_partition_erase_range(partition, 0, partition->size));
ESP_LOGI(PARTITION_NAME, "erase partition!");
uint8_t *tek_zeros = calloc(ENA_STORAGE_TEK_LENGTH + 1, sizeof(uint8_t));
ena_storage_write(ENA_STORAGE_TEK_COUNT_ADDRESS, tek_zeros, ENA_STORAGE_TEK_LENGTH + 1);
uint8_t *temp_detection_zeros = calloc(ENA_STORAGE_DETECTION_LENGTH + 4, sizeof(uint8_t));
ena_storage_write(ENA_STORAGE_TEMP_DETECTIONS_COUNT_ADDRESS, temp_detection_zeros, ENA_STORAGE_DETECTION_LENGTH + 4);
uint8_t *detection_zeros = calloc(ENA_STORAGE_DETECTION_LENGTH + 4, sizeof(uint8_t));
ena_storage_write(ENA_STORAGE_DETECTIONS_COUNT_ADDRESS, detection_zeros, ENA_STORAGE_DETECTION_LENGTH + 4);
ESP_LOGD(ENA_STORAGE_LOG, "END ena_storage_erase");
}
void ena_storage_dump_hash_array(uint8_t data[], size_t size)
{
for (int i = 0; i < size; i++)
{
if (i == 0)
{
printf("%0x", data[i]);
}
else
{
printf(" %0x", data[i]);
}
}
}
void ena_storage_dump_tek(void)
{
uint32_t timestamp;
uint8_t tek[ENA_KEY_LENGTH] = {0};
uint8_t tek_count = ena_storage_read_u8(ENA_STORAGE_TEK_COUNT_ADDRESS);
ESP_LOGD(ENA_STORAGE_LOG, "%u TEKs\n", tek_count);
printf("#,enin,tek\n");
for (int i = 0; i < tek_count; i++)
{
size_t address = ENA_STORAGE_TEK_START_ADDRESS + i * ENA_STORAGE_TEK_LENGTH;
timestamp = ena_storage_read_u32(address);
ena_storage_read(address + 4, tek, ENA_KEY_LENGTH);
printf("%d,%u,", i, timestamp);
ena_storage_dump_hash_array(tek, ENA_KEY_LENGTH);
printf("\n");
}
}
void ena_storage_dump_temp_detections(void)
{
uint32_t timestamp;
uint8_t rpi[ENA_KEY_LENGTH] = {0};
uint8_t aem[ENA_AEM_METADATA_LENGTH] = {0};
int rssi;
uint32_t detection_count = ena_storage_read_u32(ENA_STORAGE_TEMP_DETECTIONS_COUNT_ADDRESS);
ESP_LOGD(ENA_STORAGE_LOG, "%u temporary detections\n", detection_count);
printf("#,timestamp,rpi,aem,rssi\n");
for (int i = 0; i < detection_count; i++)
{
ena_storage_read_temp_detection(i, &timestamp, rpi, aem, &rssi);
printf("%d,%u,", i, timestamp);
ena_storage_dump_hash_array(rpi, ENA_KEY_LENGTH);
printf(",");
ena_storage_dump_hash_array(aem, ENA_AEM_METADATA_LENGTH);
printf(",%d\n", rssi);
}
}
void ena_storage_dump_detections(void)
{
uint32_t enin;
uint8_t rpi[ENA_KEY_LENGTH] = {0};
uint8_t aem[ENA_AEM_METADATA_LENGTH] = {0};
int rssi;
uint32_t detection_count = ena_storage_read_u32(ENA_STORAGE_DETECTIONS_COUNT_ADDRESS);
ESP_LOGD(ENA_STORAGE_LOG, "%u detections\n", detection_count);
printf("#,enin,rpi,aem,rssi\n");
for (int i = 0; i < detection_count; i++)
{
ena_storage_read_detection(i, &enin, rpi, aem, &rssi);
printf("%d,%u,", i, enin);
ena_storage_dump_hash_array(rpi, ENA_KEY_LENGTH);
printf(",");
ena_storage_dump_hash_array(aem, ENA_AEM_METADATA_LENGTH);
printf(",%d\n", rssi);
}
}
Reference in New Issue View Git Blame Copy Permalink