mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-12 19:10:27 +03:00
Merge pull request #7333 from mluessi/ml_config_extflash
Add ability to store configuration in external SPI flash
This commit is contained in:
commit
b55bc46f12
21 changed files with 318 additions and 30 deletions
|
@ -19,6 +19,8 @@ main_sources(STM32F3_SRC
|
||||||
target/system_stm32f30x.c
|
target/system_stm32f30x.c
|
||||||
|
|
||||||
config/config_streamer_stm32f3.c
|
config/config_streamer_stm32f3.c
|
||||||
|
config/config_streamer_ram.c
|
||||||
|
config/config_streamer_extflash.c
|
||||||
|
|
||||||
drivers/adc_stm32f30x.c
|
drivers/adc_stm32f30x.c
|
||||||
drivers/bus_i2c_stm32f30x.c
|
drivers/bus_i2c_stm32f30x.c
|
||||||
|
|
|
@ -36,6 +36,8 @@ main_sources(STM32F4_SRC
|
||||||
target/system_stm32f4xx.c
|
target/system_stm32f4xx.c
|
||||||
|
|
||||||
config/config_streamer_stm32f4.c
|
config/config_streamer_stm32f4.c
|
||||||
|
config/config_streamer_ram.c
|
||||||
|
config/config_streamer_extflash.c
|
||||||
|
|
||||||
drivers/adc_stm32f4xx.c
|
drivers/adc_stm32f4xx.c
|
||||||
drivers/adc_stm32f4xx.c
|
drivers/adc_stm32f4xx.c
|
||||||
|
|
|
@ -61,6 +61,8 @@ main_sources(STM32F7_SRC
|
||||||
target/system_stm32f7xx.c
|
target/system_stm32f7xx.c
|
||||||
|
|
||||||
config/config_streamer_stm32f7.c
|
config/config_streamer_stm32f7.c
|
||||||
|
config/config_streamer_ram.c
|
||||||
|
config/config_streamer_extflash.c
|
||||||
|
|
||||||
drivers/adc_stm32f7xx.c
|
drivers/adc_stm32f7xx.c
|
||||||
drivers/bus_i2c_hal.c
|
drivers/bus_i2c_hal.c
|
||||||
|
|
|
@ -142,6 +142,8 @@ main_sources(STM32H7_SRC
|
||||||
target/system_stm32h7xx.c
|
target/system_stm32h7xx.c
|
||||||
|
|
||||||
config/config_streamer_stm32h7.c
|
config/config_streamer_stm32h7.c
|
||||||
|
config/config_streamer_ram.c
|
||||||
|
config/config_streamer_extflash.c
|
||||||
|
|
||||||
drivers/adc_stm32h7xx.c
|
drivers/adc_stm32h7xx.c
|
||||||
drivers/bus_i2c_hal.c
|
drivers/bus_i2c_hal.c
|
||||||
|
@ -217,4 +219,4 @@ macro(define_target_stm32h7 subfamily size)
|
||||||
endfunction()
|
endfunction()
|
||||||
endmacro()
|
endmacro()
|
||||||
|
|
||||||
define_target_stm32h7(43 i)
|
define_target_stm32h7(43 i)
|
||||||
|
|
|
@ -53,8 +53,10 @@
|
||||||
|
|
||||||
#ifdef STM32H7
|
#ifdef STM32H7
|
||||||
#define DMA_RAM __attribute__ ((section(".DMA_RAM")))
|
#define DMA_RAM __attribute__ ((section(".DMA_RAM")))
|
||||||
|
#define SLOW_RAM __attribute__ ((section(".SLOW_RAM")))
|
||||||
#else
|
#else
|
||||||
#define DMA_RAM
|
#define DMA_RAM
|
||||||
|
#define SLOW_RAM
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#define STATIC_FASTRAM static FASTRAM
|
#define STATIC_FASTRAM static FASTRAM
|
||||||
|
|
|
@ -25,12 +25,14 @@
|
||||||
#include "build/build_config.h"
|
#include "build/build_config.h"
|
||||||
|
|
||||||
#include "common/crc.h"
|
#include "common/crc.h"
|
||||||
|
#include "common/utils.h"
|
||||||
|
|
||||||
#include "config/config_eeprom.h"
|
#include "config/config_eeprom.h"
|
||||||
#include "config/config_streamer.h"
|
#include "config/config_streamer.h"
|
||||||
#include "config/parameter_group.h"
|
#include "config/parameter_group.h"
|
||||||
|
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/flash.h"
|
||||||
|
|
||||||
#include "fc/config.h"
|
#include "fc/config.h"
|
||||||
|
|
||||||
|
@ -76,6 +78,31 @@ typedef struct {
|
||||||
uint32_t word;
|
uint32_t word;
|
||||||
} PG_PACKED packingTest_t;
|
} PG_PACKED packingTest_t;
|
||||||
|
|
||||||
|
#if defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
bool loadEEPROMFromExternalFlash(void)
|
||||||
|
{
|
||||||
|
const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
|
||||||
|
const flashGeometry_t *flashGeometry = flashGetGeometry();
|
||||||
|
|
||||||
|
uint32_t flashStartAddress = flashPartition->startSector * flashGeometry->sectorSize;
|
||||||
|
|
||||||
|
uint32_t totalBytesRead = 0;
|
||||||
|
int bytesRead = 0;
|
||||||
|
|
||||||
|
bool success = false;
|
||||||
|
|
||||||
|
do {
|
||||||
|
bytesRead = flashReadBytes(flashStartAddress + totalBytesRead, &eepromData[totalBytesRead], EEPROM_SIZE - totalBytesRead);
|
||||||
|
if (bytesRead > 0) {
|
||||||
|
totalBytesRead += bytesRead;
|
||||||
|
success = (totalBytesRead == EEPROM_SIZE);
|
||||||
|
}
|
||||||
|
} while (!success && bytesRead > 0);
|
||||||
|
|
||||||
|
return success;
|
||||||
|
}
|
||||||
|
#endif /* defined(CONFIG_IN_EXTERNAL_FLASH) */
|
||||||
|
|
||||||
void initEEPROM(void)
|
void initEEPROM(void)
|
||||||
{
|
{
|
||||||
// Verify that this architecture packs as expected.
|
// Verify that this architecture packs as expected.
|
||||||
|
@ -86,6 +113,14 @@ void initEEPROM(void)
|
||||||
BUILD_BUG_ON(sizeof(configHeader_t) != 1);
|
BUILD_BUG_ON(sizeof(configHeader_t) != 1);
|
||||||
BUILD_BUG_ON(sizeof(configFooter_t) != 2);
|
BUILD_BUG_ON(sizeof(configFooter_t) != 2);
|
||||||
BUILD_BUG_ON(sizeof(configRecord_t) != 6);
|
BUILD_BUG_ON(sizeof(configRecord_t) != 6);
|
||||||
|
|
||||||
|
#if defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
bool eepromLoaded = loadEEPROMFromExternalFlash();
|
||||||
|
if (!eepromLoaded) {
|
||||||
|
// Flash read failed - just die now
|
||||||
|
failureMode(FAILURE_FLASH_READ_FAILED);
|
||||||
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
static uint16_t updateCRC(uint16_t crc, const void *data, uint32_t length)
|
static uint16_t updateCRC(uint16_t crc, const void *data, uint32_t length)
|
||||||
|
@ -217,7 +252,9 @@ static bool writeSettingsToEEPROM(void)
|
||||||
.format = EEPROM_CONF_VERSION,
|
.format = EEPROM_CONF_VERSION,
|
||||||
};
|
};
|
||||||
|
|
||||||
config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header));
|
if (config_streamer_write(&streamer, (uint8_t *)&header, sizeof(header)) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
uint16_t crc = updateCRC(0, (uint8_t *)&header, sizeof(header));
|
uint16_t crc = updateCRC(0, (uint8_t *)&header, sizeof(header));
|
||||||
PG_FOREACH(reg) {
|
PG_FOREACH(reg) {
|
||||||
const uint16_t regSize = pgSize(reg);
|
const uint16_t regSize = pgSize(reg);
|
||||||
|
@ -231,9 +268,13 @@ static bool writeSettingsToEEPROM(void)
|
||||||
if (pgIsSystem(reg)) {
|
if (pgIsSystem(reg)) {
|
||||||
// write the only instance
|
// write the only instance
|
||||||
record.flags |= CR_CLASSICATION_SYSTEM;
|
record.flags |= CR_CLASSICATION_SYSTEM;
|
||||||
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
|
if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
|
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
|
||||||
config_streamer_write(&streamer, reg->address, regSize);
|
if (config_streamer_write(&streamer, reg->address, regSize) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
crc = updateCRC(crc, reg->address, regSize);
|
crc = updateCRC(crc, reg->address, regSize);
|
||||||
} else {
|
} else {
|
||||||
// write one instance for each profile
|
// write one instance for each profile
|
||||||
|
@ -241,10 +282,14 @@ static bool writeSettingsToEEPROM(void)
|
||||||
record.flags = 0;
|
record.flags = 0;
|
||||||
|
|
||||||
record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
|
record.flags |= ((profileIndex + 1) & CR_CLASSIFICATION_MASK);
|
||||||
config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record));
|
if (config_streamer_write(&streamer, (uint8_t *)&record, sizeof(record)) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
|
crc = updateCRC(crc, (uint8_t *)&record, sizeof(record));
|
||||||
const uint8_t *address = reg->address + (regSize * profileIndex);
|
const uint8_t *address = reg->address + (regSize * profileIndex);
|
||||||
config_streamer_write(&streamer, address, regSize);
|
if (config_streamer_write(&streamer, address, regSize) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
crc = updateCRC(crc, address, regSize);
|
crc = updateCRC(crc, address, regSize);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -254,13 +299,19 @@ static bool writeSettingsToEEPROM(void)
|
||||||
.terminator = 0,
|
.terminator = 0,
|
||||||
};
|
};
|
||||||
|
|
||||||
config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer));
|
if (config_streamer_write(&streamer, (uint8_t *)&footer, sizeof(footer)) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
crc = updateCRC(crc, (uint8_t *)&footer, sizeof(footer));
|
crc = updateCRC(crc, (uint8_t *)&footer, sizeof(footer));
|
||||||
|
|
||||||
// append checksum now
|
// append checksum now
|
||||||
config_streamer_write(&streamer, (uint8_t *)&crc, sizeof(crc));
|
if (config_streamer_write(&streamer, (uint8_t *)&crc, sizeof(crc)) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
config_streamer_flush(&streamer);
|
if (config_streamer_flush(&streamer) < 0) {
|
||||||
|
return false;
|
||||||
|
}
|
||||||
|
|
||||||
bool success = config_streamer_finish(&streamer) == 0;
|
bool success = config_streamer_finish(&streamer) == 0;
|
||||||
|
|
||||||
|
@ -274,6 +325,10 @@ void writeConfigToEEPROM(void)
|
||||||
for (int attempt = 0; attempt < 3 && !success; attempt++) {
|
for (int attempt = 0; attempt < 3 && !success; attempt++) {
|
||||||
if (writeSettingsToEEPROM()) {
|
if (writeSettingsToEEPROM()) {
|
||||||
success = true;
|
success = true;
|
||||||
|
#ifdef CONFIG_IN_EXTERNAL_FLASH
|
||||||
|
// copy it back from flash to the in-memory buffer.
|
||||||
|
success = loadEEPROMFromExternalFlash();
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -19,6 +19,11 @@
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "config/config_streamer.h"
|
#include "config/config_streamer.h"
|
||||||
|
#include "build/build_config.h"
|
||||||
|
|
||||||
|
#if !defined(CONFIG_IN_FLASH)
|
||||||
|
SLOW_RAM uint8_t eepromData[EEPROM_SIZE];
|
||||||
|
#endif
|
||||||
|
|
||||||
// Helper functions
|
// Helper functions
|
||||||
extern void config_streamer_impl_unlock(void);
|
extern void config_streamer_impl_unlock(void);
|
||||||
|
@ -35,6 +40,7 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
|
||||||
// base must start at FLASH_PAGE_SIZE boundary when using embedded flash.
|
// base must start at FLASH_PAGE_SIZE boundary when using embedded flash.
|
||||||
c->address = base;
|
c->address = base;
|
||||||
c->size = size;
|
c->size = size;
|
||||||
|
c->end = base + size;
|
||||||
if (!c->unlocked) {
|
if (!c->unlocked) {
|
||||||
config_streamer_impl_unlock();
|
config_streamer_impl_unlock();
|
||||||
c->unlocked = true;
|
c->unlocked = true;
|
||||||
|
@ -44,6 +50,11 @@ void config_streamer_start(config_streamer_t *c, uintptr_t base, int size)
|
||||||
|
|
||||||
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
|
int config_streamer_write(config_streamer_t *c, const uint8_t *p, uint32_t size)
|
||||||
{
|
{
|
||||||
|
if ((c->address + size) > c->end) {
|
||||||
|
// trying to write past end of streamer
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) {
|
for (const uint8_t *pat = p; pat != (uint8_t*)p + size; pat++) {
|
||||||
c->buffer.b[c->at++] = *pat;
|
c->buffer.b[c->at++] = *pat;
|
||||||
|
|
||||||
|
@ -63,6 +74,9 @@ int config_streamer_status(config_streamer_t *c)
|
||||||
int config_streamer_flush(config_streamer_t *c)
|
int config_streamer_flush(config_streamer_t *c)
|
||||||
{
|
{
|
||||||
if (c->at != 0) {
|
if (c->at != 0) {
|
||||||
|
if ((c->address + c->at) > c->end) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at);
|
memset(c->buffer.b + c->at, 0, sizeof(c->buffer) - c->at);
|
||||||
c->err = config_streamer_impl_write_word(c, &c->buffer.w);
|
c->err = config_streamer_impl_write_word(c, &c->buffer.w);
|
||||||
c->at = 0;
|
c->at = 0;
|
||||||
|
|
|
@ -20,10 +20,15 @@
|
||||||
#include <stdint.h>
|
#include <stdint.h>
|
||||||
#include <stdbool.h>
|
#include <stdbool.h>
|
||||||
|
|
||||||
|
#include "drivers/flash_m25p16.h"
|
||||||
|
|
||||||
// Streams data out to the EEPROM, padding to the write size as
|
// Streams data out to the EEPROM, padding to the write size as
|
||||||
// needed, and updating the checksum as it goes.
|
// needed, and updating the checksum as it goes.
|
||||||
|
|
||||||
#if defined(STM32H7)
|
#ifdef CONFIG_IN_EXTERNAL_FLASH
|
||||||
|
#define CONFIG_STREAMER_BUFFER_SIZE M25P16_PAGESIZE // Must match flash device page size
|
||||||
|
typedef uint32_t config_streamer_buffer_align_type_t;
|
||||||
|
#elif defined(STM32H7)
|
||||||
#define CONFIG_STREAMER_BUFFER_SIZE 32 // Flash word = 256-bits
|
#define CONFIG_STREAMER_BUFFER_SIZE 32 // Flash word = 256-bits
|
||||||
typedef uint64_t config_streamer_buffer_align_type_t;
|
typedef uint64_t config_streamer_buffer_align_type_t;
|
||||||
#else
|
#else
|
||||||
|
@ -33,6 +38,7 @@ typedef uint32_t config_streamer_buffer_align_type_t;
|
||||||
|
|
||||||
typedef struct config_streamer_s {
|
typedef struct config_streamer_s {
|
||||||
uintptr_t address;
|
uintptr_t address;
|
||||||
|
uintptr_t end;
|
||||||
int size;
|
int size;
|
||||||
union {
|
union {
|
||||||
uint8_t b[CONFIG_STREAMER_BUFFER_SIZE];
|
uint8_t b[CONFIG_STREAMER_BUFFER_SIZE];
|
||||||
|
|
77
src/main/config/config_streamer_extflash.c
Normal file
77
src/main/config/config_streamer_extflash.c
Normal file
|
@ -0,0 +1,77 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* Cleanflight is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
#include "platform.h"
|
||||||
|
#include "drivers/system.h"
|
||||||
|
#include "drivers/flash.h"
|
||||||
|
#include "config/config_streamer.h"
|
||||||
|
|
||||||
|
#if defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
|
||||||
|
static bool streamerLocked = true;
|
||||||
|
|
||||||
|
void config_streamer_impl_unlock(void)
|
||||||
|
{
|
||||||
|
const flashGeometry_t *flashGeometry = flashGetGeometry();
|
||||||
|
if (flashGeometry->pageSize == CONFIG_STREAMER_BUFFER_SIZE) {
|
||||||
|
// streamer needs to buffer exactly one flash page
|
||||||
|
streamerLocked = false;
|
||||||
|
}
|
||||||
|
}
|
||||||
|
|
||||||
|
void config_streamer_impl_lock(void)
|
||||||
|
{
|
||||||
|
streamerLocked = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer_align_type_t *buffer)
|
||||||
|
{
|
||||||
|
if (streamerLocked) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t dataOffset = (uint32_t)(c->address - (uintptr_t)&eepromData[0]);
|
||||||
|
|
||||||
|
const flashPartition_t *flashPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_CONFIG);
|
||||||
|
const flashGeometry_t *flashGeometry = flashGetGeometry();
|
||||||
|
|
||||||
|
uint32_t flashStartAddress = flashPartition->startSector * flashGeometry->sectorSize;
|
||||||
|
uint32_t flashOverflowAddress = ((flashPartition->endSector + 1) * flashGeometry->sectorSize); // +1 to sector for inclusive
|
||||||
|
|
||||||
|
uint32_t flashAddress = flashStartAddress + dataOffset;
|
||||||
|
if (flashAddress + CONFIG_STREAMER_BUFFER_SIZE > flashOverflowAddress) {
|
||||||
|
return -2; // address is past end of partition
|
||||||
|
}
|
||||||
|
|
||||||
|
uint32_t flashSectorSize = flashGeometry->sectorSize;
|
||||||
|
|
||||||
|
if (flashAddress % flashSectorSize == 0) {
|
||||||
|
flashEraseSector(flashAddress);
|
||||||
|
}
|
||||||
|
|
||||||
|
if (flashPageProgram(flashAddress, (uint8_t *)buffer, CONFIG_STREAMER_BUFFER_SIZE) == flashAddress) {
|
||||||
|
// returned same address: programming failed
|
||||||
|
return -3;
|
||||||
|
}
|
||||||
|
|
||||||
|
c->address += CONFIG_STREAMER_BUFFER_SIZE;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
61
src/main/config/config_streamer_ram.c
Normal file
61
src/main/config/config_streamer_ram.c
Normal file
|
@ -0,0 +1,61 @@
|
||||||
|
/*
|
||||||
|
* This file is part of Cleanflight.
|
||||||
|
*
|
||||||
|
* Cleanflight is free software: you can redistribute it and/or modify
|
||||||
|
* it under the terms of the GNU General Public License as published by
|
||||||
|
* the Free Software Foundation, either version 3 of the License, or
|
||||||
|
* (at your option) any later version.
|
||||||
|
*
|
||||||
|
* Cleanflight is distributed in the hope that it will be useful,
|
||||||
|
* but WITHOUT ANY WARRANTY; without even the implied warranty of
|
||||||
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
||||||
|
* GNU General Public License for more details.
|
||||||
|
*
|
||||||
|
* You should have received a copy of the GNU General Public License
|
||||||
|
* along with Cleanflight. If not, see <http://www.gnu.org/licenses/>.
|
||||||
|
*/
|
||||||
|
|
||||||
|
#include <string.h>
|
||||||
|
#include "platform.h"
|
||||||
|
#include "drivers/system.h"
|
||||||
|
#include "config/config_streamer.h"
|
||||||
|
|
||||||
|
#if defined(CONFIG_IN_RAM)
|
||||||
|
|
||||||
|
static bool streamerLocked = true;
|
||||||
|
|
||||||
|
void config_streamer_impl_unlock(void)
|
||||||
|
{
|
||||||
|
streamerLocked = false;
|
||||||
|
}
|
||||||
|
|
||||||
|
void config_streamer_impl_lock(void)
|
||||||
|
{
|
||||||
|
streamerLocked = true;
|
||||||
|
}
|
||||||
|
|
||||||
|
int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer_align_type_t *buffer)
|
||||||
|
{
|
||||||
|
if (streamerLocked) {
|
||||||
|
return -1;
|
||||||
|
}
|
||||||
|
|
||||||
|
if (c->address == (uintptr_t)&eepromData[0]) {
|
||||||
|
memset(eepromData, 0, sizeof(eepromData));
|
||||||
|
}
|
||||||
|
|
||||||
|
config_streamer_buffer_align_type_t *destAddr = (config_streamer_buffer_align_type_t *)c->address;
|
||||||
|
config_streamer_buffer_align_type_t *srcAddr = buffer;
|
||||||
|
|
||||||
|
uint8_t nRows = CONFIG_STREAMER_BUFFER_SIZE / sizeof(config_streamer_buffer_align_type_t);
|
||||||
|
|
||||||
|
do {
|
||||||
|
*destAddr++ = *srcAddr++;
|
||||||
|
} while(--nRows != 0);
|
||||||
|
|
||||||
|
c->address += CONFIG_STREAMER_BUFFER_SIZE;
|
||||||
|
|
||||||
|
return 0;
|
||||||
|
}
|
||||||
|
|
||||||
|
#endif
|
|
@ -20,7 +20,7 @@
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "config/config_streamer.h"
|
#include "config/config_streamer.h"
|
||||||
|
|
||||||
#if defined(STM32F3)
|
#if defined(STM32F3) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
|
||||||
#define FLASH_PAGE_SIZE 0x800
|
#define FLASH_PAGE_SIZE 0x800
|
||||||
|
|
||||||
|
@ -57,4 +57,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "config/config_streamer.h"
|
#include "config/config_streamer.h"
|
||||||
|
|
||||||
#if defined(STM32F4)
|
#if defined(STM32F4) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
|
||||||
/*
|
/*
|
||||||
Sector 0 0x08000000 - 0x08003FFF 16 Kbytes
|
Sector 0 0x08000000 - 0x08003FFF 16 Kbytes
|
||||||
|
@ -103,4 +103,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "config/config_streamer.h"
|
#include "config/config_streamer.h"
|
||||||
|
|
||||||
#if defined(STM32F7)
|
#if defined(STM32F7) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
|
||||||
#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx)
|
#if defined(STM32F745xx) || defined(STM32F746xx) || defined(STM32F765xx)
|
||||||
/*
|
/*
|
||||||
|
@ -138,4 +138,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -20,7 +20,7 @@
|
||||||
#include "drivers/system.h"
|
#include "drivers/system.h"
|
||||||
#include "config/config_streamer.h"
|
#include "config/config_streamer.h"
|
||||||
|
|
||||||
#if defined(STM32H7)
|
#if defined(STM32H7) && !defined(CONFIG_IN_RAM) && !defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
|
||||||
#if defined(STM32H743xx)
|
#if defined(STM32H743xx)
|
||||||
/* Sectors 0-7 of 128K each */
|
/* Sectors 0-7 of 128K each */
|
||||||
|
@ -96,4 +96,4 @@ int config_streamer_impl_write_word(config_streamer_t *c, config_streamer_buffer
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -196,7 +196,15 @@ static void flashConfigurePartitions(void)
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#if defined(CONFIG_IN_EXTERNAL_FLASH)
|
#if defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
createPartition(FLASH_PARTITION_TYPE_CONFIG, EEPROM_SIZE, &endSector);
|
uint32_t configSize = EEPROM_SIZE;
|
||||||
|
flashSector_t configSectors = (configSize / flashGeometry->sectorSize);
|
||||||
|
|
||||||
|
if (configSize % flashGeometry->sectorSize > 0) {
|
||||||
|
configSectors++; // needs a portion of a sector.
|
||||||
|
}
|
||||||
|
configSize = configSectors * flashGeometry->sectorSize;
|
||||||
|
|
||||||
|
createPartition(FLASH_PARTITION_TYPE_CONFIG, configSize, &endSector);
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
#ifdef USE_FLASHFS
|
#ifdef USE_FLASHFS
|
||||||
|
|
|
@ -68,6 +68,7 @@ static flashGeometry_t geometry = {.pageSize = M25P16_PAGESIZE};
|
||||||
|
|
||||||
static busDevice_t * busDev = NULL;
|
static busDevice_t * busDev = NULL;
|
||||||
static bool isLargeFlash = false;
|
static bool isLargeFlash = false;
|
||||||
|
static uint32_t timeoutAt = 0;
|
||||||
|
|
||||||
/*
|
/*
|
||||||
* Whether we've performed an action that could have made the device busy for writes.
|
* Whether we've performed an action that could have made the device busy for writes.
|
||||||
|
@ -114,15 +115,30 @@ bool m25p16_isReady(void)
|
||||||
return !couldBeBusy;
|
return !couldBeBusy;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
static void m25p16_setTimeout(uint32_t timeoutMillis)
|
||||||
|
{
|
||||||
|
uint32_t now = millis();
|
||||||
|
timeoutAt = now + timeoutMillis;
|
||||||
|
}
|
||||||
|
|
||||||
bool m25p16_waitForReady(uint32_t timeoutMillis)
|
bool m25p16_waitForReady(uint32_t timeoutMillis)
|
||||||
{
|
{
|
||||||
uint32_t time = millis();
|
uint32_t timeoutAtUse;
|
||||||
|
if (timeoutMillis == 0) {
|
||||||
|
timeoutAtUse = timeoutAt;
|
||||||
|
}
|
||||||
|
else {
|
||||||
|
timeoutAtUse = millis() + timeoutMillis;
|
||||||
|
}
|
||||||
|
|
||||||
while (!m25p16_isReady()) {
|
while (!m25p16_isReady()) {
|
||||||
if (timeoutMillis && (millis() - time > timeoutMillis)) {
|
uint32_t now = millis();
|
||||||
|
if (now >= timeoutAtUse) {
|
||||||
return false;
|
return false;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
timeoutAt = 0;
|
||||||
return true;
|
return true;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -216,10 +232,6 @@ static bool m25p16_readIdentification(void)
|
||||||
*/
|
*/
|
||||||
bool m25p16_init(int flashNumToUse)
|
bool m25p16_init(int flashNumToUse)
|
||||||
{
|
{
|
||||||
if (busDev) {
|
|
||||||
return true;
|
|
||||||
}
|
|
||||||
|
|
||||||
busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH);
|
busDev = busDeviceInit(BUSTYPE_SPI, DEVHW_M25P16, flashNumToUse, OWNER_FLASH);
|
||||||
if (busDev == NULL) {
|
if (busDev == NULL) {
|
||||||
return false;
|
return false;
|
||||||
|
@ -252,20 +264,28 @@ void m25p16_eraseSector(uint32_t address)
|
||||||
|
|
||||||
m25p16_setCommandAddress(&out[1], address, isLargeFlash);
|
m25p16_setCommandAddress(&out[1], address, isLargeFlash);
|
||||||
|
|
||||||
m25p16_waitForReady(SECTOR_ERASE_TIMEOUT_MILLIS);
|
if (!m25p16_waitForReady(0)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
m25p16_writeEnable();
|
m25p16_writeEnable();
|
||||||
|
|
||||||
busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4);
|
busTransfer(busDev, NULL, out, isLargeFlash ? 5 : 4);
|
||||||
|
|
||||||
|
m25p16_setTimeout(SECTOR_ERASE_TIMEOUT_MILLIS);
|
||||||
}
|
}
|
||||||
|
|
||||||
void m25p16_eraseCompletely(void)
|
void m25p16_eraseCompletely(void)
|
||||||
{
|
{
|
||||||
m25p16_waitForReady(BULK_ERASE_TIMEOUT_MILLIS);
|
if (!m25p16_waitForReady(0)) {
|
||||||
|
return;
|
||||||
|
}
|
||||||
|
|
||||||
m25p16_writeEnable();
|
m25p16_writeEnable();
|
||||||
|
|
||||||
m25p16_performOneByteCommand(M25P16_INSTRUCTION_BULK_ERASE);
|
m25p16_performOneByteCommand(M25P16_INSTRUCTION_BULK_ERASE);
|
||||||
|
|
||||||
|
m25p16_setTimeout(BULK_ERASE_TIMEOUT_MILLIS);
|
||||||
}
|
}
|
||||||
|
|
||||||
/**
|
/**
|
||||||
|
@ -294,12 +314,17 @@ uint32_t m25p16_pageProgram(uint32_t address, const uint8_t *data, int length)
|
||||||
|
|
||||||
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
|
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
|
||||||
|
|
||||||
m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS);
|
if (!m25p16_waitForReady(0)) {
|
||||||
|
// return same address to indicate timeout
|
||||||
|
return address;
|
||||||
|
}
|
||||||
|
|
||||||
m25p16_writeEnable();
|
m25p16_writeEnable();
|
||||||
|
|
||||||
busTransferMultiple(busDev, txn, 2);
|
busTransferMultiple(busDev, txn, 2);
|
||||||
|
|
||||||
|
m25p16_setTimeout(DEFAULT_TIMEOUT_MILLIS);
|
||||||
|
|
||||||
return address + length;
|
return address + length;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -322,7 +347,7 @@ int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length)
|
||||||
|
|
||||||
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
|
m25p16_setCommandAddress(&command[1], address, isLargeFlash);
|
||||||
|
|
||||||
if (!m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS)) {
|
if (!m25p16_waitForReady(0)) {
|
||||||
return 0;
|
return 0;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|
|
@ -30,7 +30,8 @@ typedef enum {
|
||||||
FAILURE_ACC_INCOMPATIBLE,
|
FAILURE_ACC_INCOMPATIBLE,
|
||||||
FAILURE_INVALID_EEPROM_CONTENTS,
|
FAILURE_INVALID_EEPROM_CONTENTS,
|
||||||
FAILURE_FLASH_WRITE_FAILED,
|
FAILURE_FLASH_WRITE_FAILED,
|
||||||
FAILURE_GYRO_INIT_FAILED
|
FAILURE_GYRO_INIT_FAILED,
|
||||||
|
FAILURE_FLASH_READ_FAILED,
|
||||||
} failureMode_e;
|
} failureMode_e;
|
||||||
|
|
||||||
// failure
|
// failure
|
||||||
|
|
|
@ -217,6 +217,13 @@ void init(void)
|
||||||
detectBrushedESC();
|
detectBrushedESC();
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#ifdef CONFIG_IN_EXTERNAL_FLASH
|
||||||
|
// Reset config to defaults. Note: Default flash config must be functional for config in external flash to work.
|
||||||
|
pgResetAll(0);
|
||||||
|
|
||||||
|
flashDeviceInitialized = flashInit();
|
||||||
|
#endif
|
||||||
|
|
||||||
initEEPROM();
|
initEEPROM();
|
||||||
ensureEEPROMContainsValidData();
|
ensureEEPROMContainsValidData();
|
||||||
readEEPROM();
|
readEEPROM();
|
||||||
|
@ -360,6 +367,11 @@ void init(void)
|
||||||
// Initialize buses
|
// Initialize buses
|
||||||
busInit();
|
busInit();
|
||||||
|
|
||||||
|
#ifdef CONFIG_IN_EXTERNAL_FLASH
|
||||||
|
// busInit re-configures the SPI pins. Init flash again so it is ready to write settings
|
||||||
|
flashDeviceInitialized = flashInit();
|
||||||
|
#endif
|
||||||
|
|
||||||
#ifdef USE_HARDWARE_REVISION_DETECTION
|
#ifdef USE_HARDWARE_REVISION_DETECTION
|
||||||
updateHardwareRevision();
|
updateHardwareRevision();
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -39,8 +39,11 @@
|
||||||
#define AVAILABLE_FIRMWARE_SPACE (FLASH_END - FIRMWARE_START_ADDRESS)
|
#define AVAILABLE_FIRMWARE_SPACE (FLASH_END - FIRMWARE_START_ADDRESS)
|
||||||
|
|
||||||
extern uint8_t __firmware_start; // set via linker
|
extern uint8_t __firmware_start; // set via linker
|
||||||
|
|
||||||
|
#if defined(CONFIG_IN_FLASH)
|
||||||
extern uint8_t __config_start;
|
extern uint8_t __config_start;
|
||||||
extern uint8_t __config_end;
|
extern uint8_t __config_end;
|
||||||
|
#endif
|
||||||
|
|
||||||
typedef struct {
|
typedef struct {
|
||||||
uint32_t magic;
|
uint32_t magic;
|
||||||
|
|
|
@ -88,3 +88,18 @@ extern uint8_t __config_end;
|
||||||
#define FILE_COMPILE_NORMAL
|
#define FILE_COMPILE_NORMAL
|
||||||
#define FILE_COMPILE_FOR_SPEED
|
#define FILE_COMPILE_FOR_SPEED
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
#if defined(CONFIG_IN_RAM) || defined(CONFIG_IN_EXTERNAL_FLASH)
|
||||||
|
#ifndef EEPROM_SIZE
|
||||||
|
#define EEPROM_SIZE 8192
|
||||||
|
#endif
|
||||||
|
extern uint8_t eepromData[EEPROM_SIZE];
|
||||||
|
#define __config_start (*eepromData)
|
||||||
|
#define __config_end (*ARRAYEND(eepromData))
|
||||||
|
#else
|
||||||
|
#ifndef CONFIG_IN_FLASH
|
||||||
|
#define CONFIG_IN_FLASH
|
||||||
|
#endif
|
||||||
|
extern uint8_t __config_start; // configured via linker script when building binaries.
|
||||||
|
extern uint8_t __config_end;
|
||||||
|
#endif
|
||||||
|
|
|
@ -56,5 +56,6 @@ MEMORY
|
||||||
|
|
||||||
REGION_ALIAS("STACKRAM", DTCM_RAM)
|
REGION_ALIAS("STACKRAM", DTCM_RAM)
|
||||||
REGION_ALIAS("FASTRAM", DTCM_RAM)
|
REGION_ALIAS("FASTRAM", DTCM_RAM)
|
||||||
|
REGION_ALIAS("SLOWRAM", D2_RAM)
|
||||||
|
|
||||||
INCLUDE "stm32_flash_h7_split.ld"
|
INCLUDE "stm32_flash_h7_split.ld"
|
Loading…
Add table
Add a link
Reference in a new issue