Merge pull request #10355 from geoffsim/MSP-VTX
[inav.git] / src / bl / bl_main.c
blob6fddc7d72fdac06579926b9bcdfe524779d4c28a
1 /*
2 * This file is part of INAV.
4 * INAV is free software. You can redistribute this software
5 * and/or modify this software under the terms of the
6 * GNU General Public License as published by the Free Software
7 * Foundation, either version 3 of the License, or (at your option)
8 * any later version.
10 * INAV is distributed in the hope that they will be
11 * useful, but WITHOUT ANY WARRANTY; without even the implied
12 * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
13 * See the GNU General Public License for more details.
15 * You should have received a copy of the GNU General Public License
16 * along with this software.
18 * If not, see <http://www.gnu.org/licenses/>.
21 #include <stdbool.h>
22 #include <stdint.h>
23 #include <string.h>
25 #include "platform.h"
27 /*#include "common/log.h"*/
28 #include "common/maths.h"
29 /*#include "common/printf.h"*/
31 #include "drivers/bus.h"
32 #include "drivers/flash.h"
33 #include "drivers/persistent.h"
34 #include "drivers/io.h"
35 #include "drivers/light_led.h"
37 #include "drivers/sdcard/sdcard.h"
39 #include "drivers/system.h"
40 #include "drivers/time.h"
42 #include "fc/firmware_update_common.h"
44 #include "io/asyncfatfs/asyncfatfs.h"
47 #if !(defined(USE_FLASHFS) || defined(USE_SDCARD))
48 #error No storage backend available
49 #endif
52 typedef struct {
53 uint16_t size;
54 uint8_t count;
55 } flashSectorDef_t;
57 #if defined(STM32F405xx)
58 #define SECTOR_COUNT 12
59 flashSectorDef_t flashSectors[] = { { 16, 4 }, { 64, 1 }, { 128, 7 }, { 0, 0 } };
61 #elif defined(STM32F722xx)
62 #define SECTOR_COUNT 8
63 flashSectorDef_t flashSectors[] = { { 16, 4 }, { 64, 1 }, { 128, 3 }, { 0, 0 } };
65 #elif defined(STM32F745xG) || defined(STM32F765xG)
66 #define SECTOR_COUNT 8
67 flashSectorDef_t flashSectors[] = { { 32, 4 }, { 128, 1 }, { 256, 3 }, { 0, 0 } };
69 #elif defined(STM32F765xI)
70 #define SECTOR_COUNT 8
71 flashSectorDef_t flashSectors[] = { { 32, 4 }, { 128, 1 }, { 256, 7 }, { 0, 0 } };
73 #elif defined(AT32F437ZMT7) || defined(AT32F437VMT7) || defined(AT32F435RMT7)
74 #define SECTOR_COUNT 1007
75 flashSectorDef_t flashSectors[] = { { 4, 1007 }, { 0, 0 } };
77 #elif defined(AT32F437ZGT7) ||defined(AT32F437VGT7) || defined(AT32F435RGT7)
78 #define SECTOR_COUNT 511
79 flashSectorDef_t flashSectors[] = { { 2, 511 }, { 0, 0 } };
81 #else
83 #error Unsupported MCU
84 #endif
86 #if defined(STM32F4)
87 #define flashLock() FLASH_Lock()
88 #define flashUnlock() FLASH_Unlock()
89 #elif defined(STM32F7)
90 #define flashLock() HAL_FLASH_Lock()
91 #define flashUnlock() HAL_FLASH_Unlock()
92 #elif defined(AT32F43x)
93 #define flashLock() flash_lock()
94 #define flashUnlock() flash_unlock()
95 #endif
97 static bool dataBackEndInitialized = false;
99 #ifdef USE_SDCARD
100 static afatfsFilePtr_t flashDataFile = NULL;
102 static void flashDataFileOpenCallback(afatfsFilePtr_t file)
104 flashDataFile = file;
106 #endif
108 static void init(void)
110 #ifdef USE_HAL_DRIVER
111 HAL_Init();
112 #endif
114 /*printfSupportInit();*/
116 systemInit();
118 __enable_irq();
120 // initialize IO (needed for all IO operations)
121 IOInitGlobal();
123 ledInit(false);
125 LED0_OFF;
126 LED1_OFF;
128 for(int x = 0; x < 10; ++x) {
129 LED0_TOGGLE;
130 LED1_TOGGLE;
131 delay(200);
136 static bool dataBackendInit(void)
138 if (dataBackEndInitialized) return true;
140 busInit();
142 #if defined(USE_SDCARD)
143 sdcardInsertionDetectInit();
144 sdcard_init();
145 afatfs_init();
147 afatfsError_e sdError = afatfs_getLastError();
148 while ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY) && ((sdError = afatfs_getLastError()) == AFATFS_ERROR_NONE)) {
149 afatfs_poll();
152 if (sdError != AFATFS_ERROR_NONE) {
153 return false;
156 #elif defined(USE_FLASHFS)
157 if (!flashInit()) {
158 return false;
161 #endif
163 dataBackEndInitialized = true;
164 return true;
167 typedef void resetHandler_t(void);
169 typedef struct isrVector_s {
170 uint32_t stackEnd;
171 resetHandler_t *resetHandler;
172 } isrVector_t;
174 static void do_jump(uint32_t address)
176 #ifdef STM32F7
177 __DSB();
178 __DMB();
179 __ISB();
180 #endif
182 volatile isrVector_t *bootloaderVector = (isrVector_t *)address;
183 __set_MSP(bootloaderVector->stackEnd);
184 bootloaderVector->resetHandler();
187 void bootloader_jump_to_app(void)
189 #if defined(AT32F43x)
190 /*Close Peripherals Clock*/
191 CRM->apb2rst = 0xFFFF;
192 CRM->apb2rst = 0;
193 CRM->apb1rst = 0xFFFF;
194 CRM->apb1rst = 0;
195 CRM->apb1en = 0;
196 CRM->apb2en = 0;
197 /*Close PLL*/
198 /* Reset SW, AHBDIV, APB1DIV, APB2DIV, ADCDIV and CLKOUT_SEL bits */
199 CRM->cfg_bit.sclksel = 0;
200 CRM->cfg_bit.ahbdiv = 0;
201 CRM->cfg_bit.apb1div = 0;
202 CRM->cfg_bit.apb2div = 0;
203 CRM->ctrl_bit.hexten = 0;
204 CRM->ctrl_bit.cfden = 0;
205 CRM->ctrl_bit.pllen = 0;
206 /* Disable all interrupts and clear pending bits */
207 CRM->clkint_bit.lickstblfc = 0;
208 CRM->clkint_bit.lextstblfc = 0;
209 CRM->clkint_bit.hickstblfc = 0;
210 CRM->clkint_bit.hextstblfc = 0;
211 CRM->clkint_bit.pllstblfc = 0;
212 CRM->clkint_bit.cfdfc = 0;
213 /*Colse Systick*/
214 SysTick->CTRL = 0;
216 #else
217 FLASH->ACR &= (~FLASH_ACR_PRFTEN);
219 #if defined(STM32F4)
220 RCC_APB1PeriphResetCmd(~0, DISABLE);
221 RCC_APB2PeriphResetCmd(~0, DISABLE);
222 #elif defined(STM32F7)
223 RCC->APB1ENR = 0;
224 RCC->APB1LPENR = 0;
225 RCC->APB2ENR = 0;
226 RCC->APB2LPENR = 0;
227 #endif
228 #endif
230 __disable_irq();
232 do_jump(FIRMWARE_START_ADDRESS);
235 // find sector specified address is in (assume than the config section doesn't span more than 1 sector)
236 // returns -1 if not found
237 int8_t mcuFlashAddressSectorIndex(uint32_t address)
239 uint32_t sectorStartAddress = FLASH_START_ADDRESS;
240 uint8_t sector = 0;
241 flashSectorDef_t *sectorDef = flashSectors;
243 do {
244 for (unsigned j = 0; j < sectorDef->count; ++j) {
245 uint32_t sectorEndAddress = sectorStartAddress + sectorDef->size * 1024;
246 if ((address >= sectorStartAddress) && (address < sectorEndAddress)) {
247 return sector;
249 sectorStartAddress = sectorEndAddress;
250 sector += 1;
252 sectorDef += 1;
253 } while (sectorDef->count);
255 return -1;
258 uint32_t mcuFlashSectorID(uint8_t sectorIndex)
260 #if defined(STM32F4)
261 if (sectorIndex < 12) {
262 return sectorIndex * 8;
263 } else {
264 return 0x80 + (sectorIndex - 12) * 8;
266 #elif defined(STM32F7)
267 return sectorIndex;
268 #elif defined(AT32F437ZMT7) || defined(AT32F437VMT7) || defined(AT32F435RMT7)
269 if (sectorIndex < 512)
271 return FLASH_START_ADDRESS + sectorIndex * 4 * 1024;
273 else
275 return FLASH_START_ADDRESS + 0x200000 + (sectorIndex-512) * 4 * 1024;
277 #elif defined(AT32F437ZGT7) ||defined(AT32F437VGT7) || defined(AT32F435RGT7)
278 return FLASH_START_ADDRESS + sectorIndex * 2 * 1024;
279 #endif
282 bool mcuFlashSectorErase(uint8_t sectorIndex)
284 #if defined(STM32F4)
285 return (FLASH_EraseSector(mcuFlashSectorID(sectorIndex), VoltageRange_3) == FLASH_COMPLETE);
286 #elif defined(STM32F7)
287 FLASH_EraseInitTypeDef EraseInitStruct = {
288 .TypeErase = FLASH_TYPEERASE_SECTORS,
289 .VoltageRange = FLASH_VOLTAGE_RANGE_3, // 2.7-3.6V
290 .NbSectors = 1
292 EraseInitStruct.Sector = mcuFlashSectorID(sectorIndex);
293 uint32_t SECTORError;
294 const HAL_StatusTypeDef status = HAL_FLASHEx_Erase(&EraseInitStruct, &SECTORError);
295 return (status == HAL_OK);
296 #elif defined(AT32F43x)
297 return (flash_sector_erase(mcuFlashSectorID(sectorIndex)) == FLASH_OPERATE_DONE);
298 #else
300 #error Unsupported MCU
301 #endif
304 bool mcuFirmwareFlashErase(bool includeConfig)
306 int8_t firmwareSectorIndex = mcuFlashAddressSectorIndex(FIRMWARE_START_ADDRESS);
307 int8_t configSectorIndex = mcuFlashAddressSectorIndex(CONFIG_START_ADDRESS);
309 if ((firmwareSectorIndex == -1) || (configSectorIndex == -1)) {
310 return false;
313 LED0_OFF;
314 LED1_ON;
315 for (unsigned i = firmwareSectorIndex; i < SECTOR_COUNT; ++i) {
316 if (includeConfig || (!includeConfig && (i != (uint8_t)configSectorIndex))) {
317 if (!mcuFlashSectorErase(i)) {
318 LED1_OFF;
319 return false;
321 LED0_TOGGLE;
325 LED1_OFF;
326 return true;
329 bool mcuFlashWriteWord(uint32_t address, uint32_t data)
331 #if defined(STM32F4)
332 const FLASH_Status status = FLASH_ProgramWord(address, data);
333 return (status == FLASH_COMPLETE);
334 #elif defined(STM32F7)
335 const HAL_StatusTypeDef status = HAL_FLASH_Program(FLASH_TYPEPROGRAM_WORD, address, (uint64_t)data);
336 return (status == HAL_OK);
337 #elif defined(AT32F43x)
338 flash_status_type status = FLASH_OPERATE_DONE;
339 status = flash_word_program(address, data);
340 return (status == FLASH_OPERATE_DONE);
341 #else
342 #error Unsupported MCU
343 #endif
346 typedef enum {
347 FLASH_OPERATION_UPDATE,
348 FLASH_OPERATION_ROLLBACK
349 } flashOperation_e;
351 #if defined(USE_SDCARD)
352 bool afatfs_fseekWorkAround(afatfsFilePtr_t file, uint32_t forward)
354 uint8_t buffer[256];
355 while (forward > 0) {
356 uint32_t bytesRead = afatfs_freadSync(file, buffer, MIN(forward, (uint16_t)256));
357 if (bytesRead < 256) {
358 return false;
360 forward -= bytesRead;
362 return true;
364 #endif
366 bool flash(flashOperation_e flashOperation)
368 if (!dataBackendInit()) {
369 return false;
372 uint32_t buffer;
373 uint32_t flashDstAddress = FIRMWARE_START_ADDRESS + sizeof(buffer); // Write the first bytes last so that we can check that the firmware has been written fully
375 #if defined(USE_SDCARD)
376 const char * const flashDataFileName = (flashOperation == FLASH_OPERATION_UPDATE ? FIRMWARE_UPDATE_FIRMWARE_FILENAME : FIRMWARE_UPDATE_BACKUP_FILENAME);
377 if ((afatfs_getFilesystemState() != AFATFS_FILESYSTEM_STATE_READY)
378 || !afatfs_fopen(flashDataFileName, "r", flashDataFileOpenCallback)
379 || (afatfs_fileSize(flashDataFile) > AVAILABLE_FIRMWARE_SPACE)) {
380 return false;
383 #elif defined(USE_FLASHFS)
384 flashPartitionType_e srcFlashPartitionType = (flashOperation == FLASH_OPERATION_UPDATE ? FLASH_PARTITION_TYPE_UPDATE_FIRMWARE : FLASH_PARTITION_TYPE_FULL_BACKUP);
385 flashPartition_t *flashDataPartition = flashPartitionFindByType(srcFlashPartitionType);
386 const flashGeometry_t *flashGeometry = flashGetGeometry();
387 uint32_t flashDataPartitionSize = (flashDataPartition->endSector - flashDataPartition->startSector + 1) * (flashGeometry->sectorSize * flashGeometry->pageSize);
388 firmwareUpdateMetadata_t updateMetadata;
390 if (!flashDataPartition || !firmwareUpdateMetadataRead(&updateMetadata)
391 || (updateMetadata.firmwareSize > flashDataPartitionSize)
392 || (updateMetadata.firmwareSize > AVAILABLE_FIRMWARE_SPACE)) {
393 return false;
396 #endif
398 flashUnlock();
399 bool flashSucceeded = false;
401 if (!mcuFirmwareFlashErase(flashOperation != FLASH_OPERATION_UPDATE)) goto flashFailed;
403 LED0_OFF;
404 LED1_OFF;
406 uint32_t counter = 0;
408 #if defined(USE_SDCARD)
410 if (afatfs_fseekSync(flashDataFile, sizeof(buffer), AFATFS_SEEK_SET) == AFATFS_OPERATION_FAILURE) {
411 goto flashFailed;
413 // Write MCU flash
414 while (!afatfs_feof(flashDataFile)) {
416 if ((flashOperation == FLASH_OPERATION_UPDATE) && (flashDstAddress == CONFIG_START_ADDRESS)) {
417 // skip config region
418 const uint32_t configSize = CONFIG_END_ADDRESS - CONFIG_START_ADDRESS;
419 /*if (afatfs_fseekSync(flashDataFile, configSize, AFATFS_SEEK_CUR) == AFATFS_OPERATION_FAILURE) {*/
420 if (!afatfs_fseekWorkAround(flashDataFile, configSize)) { // workaround fseek bug, should be ^^^^^^^^^
421 goto flashFailed;
423 flashDstAddress += configSize;
426 afatfs_freadSync(flashDataFile, (uint8_t *)&buffer, sizeof(buffer));
427 // Write SD card files to MCU flash
428 if (!mcuFlashWriteWord(flashDstAddress, buffer)) {
429 goto flashFailed;
432 flashDstAddress += sizeof(buffer);
434 if (++counter % (10*1024/4) == 0) {
435 LED0_TOGGLE;
436 LED1_TOGGLE;
441 if ((afatfs_fseekSync(flashDataFile, 0, AFATFS_SEEK_SET) == AFATFS_OPERATION_FAILURE)
442 || (afatfs_freadSync(flashDataFile, (uint8_t *)&buffer, sizeof(buffer)) != sizeof(buffer))) {
443 goto flashFailed;
446 #elif defined(USE_FLASHFS)
447 const uint32_t flashSrcStartAddress = flashDataPartition->startSector * flashGeometry->sectorSize;
448 uint32_t flashSrcAddress = flashSrcStartAddress + sizeof(buffer);
449 const uint32_t flashDstEndAddress = (flashOperation == FLASH_OPERATION_UPDATE ? FIRMWARE_START_ADDRESS + updateMetadata.firmwareSize : FLASH_END);
451 while (flashDstAddress < flashDstEndAddress) {
453 if ((flashOperation == FLASH_OPERATION_UPDATE) && (flashDstAddress == CONFIG_START_ADDRESS)) {
454 // skip config region
455 const uint32_t configSize = CONFIG_END_ADDRESS - CONFIG_START_ADDRESS;
456 flashSrcAddress += configSize;
457 flashDstAddress += configSize;
459 if (flashDstAddress >= flashDstEndAddress) {
460 goto flashFailed;
464 flashReadBytes(flashSrcAddress, (uint8_t*)&buffer, sizeof(buffer));
465 if (!mcuFlashWriteWord(flashDstAddress, buffer)) {
466 goto flashFailed;
469 flashSrcAddress += sizeof(buffer);
470 flashDstAddress += sizeof(buffer);
472 if (++counter % (10*1024/4) == 0) {
473 LED0_TOGGLE;
474 LED1_TOGGLE;
479 flashReadBytes(flashSrcStartAddress, (uint8_t*)&buffer, sizeof(buffer));
481 #endif
483 if (!mcuFlashWriteWord(FIRMWARE_START_ADDRESS, buffer)) {
484 goto flashFailed;
487 flashSucceeded = true;
489 flashFailed:
491 flashLock();
493 LED0_OFF;
494 LED1_OFF;
496 return flashSucceeded;
499 #if defined(USE_FLASHFS)
500 // Erase falsh
501 bool dataflashChipEraseUpdatePartition(void)
503 flashPartition_t *flashDataPartition = flashPartitionFindByType(FLASH_PARTITION_TYPE_UPDATE_FIRMWARE);
505 if (!flashDataPartition) {
506 return false;
509 const flashGeometry_t *flashGeometry = flashGetGeometry();
511 LED0_OFF;
513 for (unsigned i = flashDataPartition->startSector; i <= flashDataPartition->endSector; i++) {
514 uint32_t flashAddress = flashGeometry->sectorSize * i;
515 flashEraseSector(flashAddress);
516 flashWaitForReady(1000);
517 LED0_TOGGLE;
520 LED0_OFF;
522 return true;
524 #endif
525 // Refresh from SD or FLASH
526 int main(void)
528 init();
530 uint32_t bootloaderRequest = persistentObjectRead(PERSISTENT_OBJECT_RESET_REASON);
531 if ((bootloaderRequest == RESET_BOOTLOADER_FIRMWARE_UPDATE) || (bootloaderRequest == RESET_BOOTLOADER_FIRMWARE_ROLLBACK)) {
532 flashOperation_e flashOperation = (bootloaderRequest == RESET_BOOTLOADER_FIRMWARE_UPDATE ? FLASH_OPERATION_UPDATE : FLASH_OPERATION_ROLLBACK);
533 const bool success = flash(flashOperation);
534 persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, success ? RESET_BOOTLOADER_FIRMWARE_UPDATE_SUCCESS : RESET_BOOTLOADER_FIRMWARE_UPDATE_FAILED);
535 } else if (*(uint32_t*)FIRMWARE_START_ADDRESS == 0xFFFFFFFF) {
536 if (!flash(FLASH_OPERATION_ROLLBACK)) {
537 LED0_OFF;
538 LED1_OFF;
540 while (true) {
541 LED0_TOGGLE;
542 LED1_TOGGLE;
543 delay(2000);
548 bootloader_jump_to_app();
550 return 0;