diff --git a/stm-firmware/safety/safety-memory.c b/stm-firmware/safety/safety-memory.c index 23d3ecb..16f6cff 100644 --- a/stm-firmware/safety/safety-memory.c +++ b/stm-firmware/safety/safety-memory.c @@ -477,11 +477,136 @@ return_value: return ret; } -int safety_memory_insert_config_override(struct config_override *config_override); -int safety_memory_get_config_override_count(uint32_t *count); +static uint32_t convert_config_override_to_word(const struct config_override *conf_override) +{ + uint32_t data = 0; -int safety_memory_get_config_override(uint32_t idx, struct config_override *config_override); + if (conf_override->type == SAFETY_MEMORY_CONFIG_OVERRIDE_WEIGHT) { + data |= 0xAA0000A2UL; + data |= ((uint32_t)conf_override->entry.weight_override.flag) << 16; + data |= ((uint32_t)conf_override->entry.weight_override.weight) << 8; + } else if (conf_override->type == SAFETY_MEMORY_CONFIG_OVERRIDE_PERSISTANCE) { + data |= 0xBB00008EUL; + data |= ((uint32_t)conf_override->entry.persistance_override.flag) << 16; + data |= ((uint32_t)conf_override->entry.persistance_override.persistance) << 8; + } + + return data; +} + +int safety_memory_insert_config_override(struct config_override *config_override) +{ + struct safety_memory_header header; + uint32_t idx; + uint32_t data; + int res; + int ret = -3; + + if (safety_memory_get_header(&header) != SAFETY_MEMORY_INIT_VALID_MEMORY) { + return -2000; + } + + if (header.config_overrides_len == 0) + return -1; + + for (idx = 0; idx < header.config_overrides_len; idx++) { + res = backup_ram_get_data(header.config_overrides_offset + idx, &data, 1UL); + if (res) + return -2; + if (data == 0UL) { + data = convert_config_override_to_word(config_override); + res = backup_ram_write_data(header.config_overrides_offset + idx, &data, 1UL); + if (res) + return -4; + res = safety_memory_gen_crc(); + if (res) + return -5; + ret = 0; + break; + } + } + + return ret; +} + +int safety_memory_get_config_override_count(uint32_t *count) +{ + struct safety_memory_header header; + uint32_t iter; + uint32_t valid_count; + uint32_t data; + int res; + + if (!count) + return -1001; + + *count = 0UL; + + if (safety_memory_get_header(&header) != SAFETY_MEMORY_INIT_VALID_MEMORY) { + return -2000; + } + + if (header.config_overrides_len == 0) + return 0; + + valid_count = 0; + for (iter = 0; iter < header.config_overrides_len; iter++) { + res = backup_ram_get_data(header.config_overrides_offset + iter, &data, 1UL); + if (res) + return -2; + + if (data != 0) + valid_count++; + else + break; + } + + *count = valid_count; + return 0; +} + +int safety_memory_get_config_override(uint32_t idx, struct config_override *config_override) +{ + struct safety_memory_header header; + uint32_t data; + int res; + + if (!config_override) + return -1002; + + if (safety_memory_get_header(&header) != SAFETY_MEMORY_INIT_VALID_MEMORY) { + return -2000; + } + + if (idx >= header.config_overrides_len) { + return -1001; + } + + res = backup_ram_get_data(header.config_overrides_offset + idx, &data, 1UL); + if (res) { + return -1; + } + + switch (data & 0xFF) { + case 0xA2: + /* Weight override */ + config_override->type = SAFETY_MEMORY_CONFIG_OVERRIDE_WEIGHT; + config_override->entry.weight_override.flag = (data & 0xFF0000UL) >> 16; + config_override->entry.weight_override.weight = (data & 0xFF00UL) >> 8; + break; + case 0x8E: + /* persistance override */ + config_override->type = SAFETY_MEMORY_CONFIG_OVERRIDE_PERSISTANCE; + config_override->entry.persistance_override.flag = (data & 0xFF0000UL) >> 16; + config_override->entry.persistance_override.persistance = (data & 0xFF00UL) >> 8; + break; + default: + return -2; + } + + return 0; +} int safety_memory_dump_base64(char *buffer, size_t buffsize, size_t *used_size) {