mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-07 16:38:30 -04:00
ccb583d092
add support for log time to block logger refactor rotation into backed. Don't start logs when erasing correct log start logic separate read and write points so that requesting log information does not corrupt the current log when starting a new log stop logging first clear the write buffer when starting a new log insert utc time when requesting info for the current log stop logging and request formats again when starting a new log cope with erase happening while we are logging keep pushing out startup messages even when format messages are done don't log to the gcs in the io thread don't start new logs in the io thread don't validate logs while erasing flush logs when stopping logging account for page header when calculating logs sizes don't return data when asked for more data than in the log optimize locking and use separate semaphore to mediate ring buffer access stop logging when the chip is full and send a notification calculate logs sizes correctly even when they wrap read log data correctly even when it wraps add stats support to block logger reset dropped when starting a new log fail logging when the chip is full refactor critical bufferspace checks increase messagewriter budget to 250us and to 300us for FMT
328 lines
8.0 KiB
C++
328 lines
8.0 KiB
C++
/*
|
|
logging to a DataFlash block based storage device on SPI
|
|
*/
|
|
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
|
|
#ifdef HAL_LOGGING_DATAFLASH
|
|
|
|
#include "AP_Logger_DataFlash.h"
|
|
#include <stdio.h>
|
|
|
|
extern const AP_HAL::HAL& hal;
|
|
|
|
#define JEDEC_WRITE_ENABLE 0x06
|
|
#define JEDEC_WRITE_DISABLE 0x04
|
|
#define JEDEC_READ_STATUS 0x05
|
|
#define JEDEC_WRITE_STATUS 0x01
|
|
#define JEDEC_READ_DATA 0x03
|
|
#define JEDEC_FAST_READ 0x0b
|
|
#define JEDEC_DEVICE_ID 0x9F
|
|
#define JEDEC_PAGE_WRITE 0x02
|
|
|
|
#define JEDEC_BULK_ERASE 0xC7
|
|
#define JEDEC_SECTOR4_ERASE 0x20 // 4k erase
|
|
#define JEDEC_BLOCK32_ERASE 0x52 // 32K erase
|
|
#define JEDEC_BLOCK64_ERASE 0xD8 // 64K erase
|
|
|
|
#define JEDEC_STATUS_BUSY 0x01
|
|
#define JEDEC_STATUS_WRITEPROTECT 0x02
|
|
#define JEDEC_STATUS_BP0 0x04
|
|
#define JEDEC_STATUS_BP1 0x08
|
|
#define JEDEC_STATUS_BP2 0x10
|
|
#define JEDEC_STATUS_TP 0x20
|
|
#define JEDEC_STATUS_SEC 0x40
|
|
#define JEDEC_STATUS_SRP0 0x80
|
|
|
|
/*
|
|
flash device IDs taken from betaflight flash_m25p16.c
|
|
|
|
Format is manufacturer, memory type, then capacity
|
|
*/
|
|
#define JEDEC_ID_MACRONIX_MX25L3206E 0xC22016
|
|
#define JEDEC_ID_MACRONIX_MX25L6406E 0xC22017
|
|
#define JEDEC_ID_MACRONIX_MX25L25635E 0xC22019
|
|
#define JEDEC_ID_MICRON_M25P16 0x202015
|
|
#define JEDEC_ID_MICRON_N25Q064 0x20BA17
|
|
#define JEDEC_ID_MICRON_N25Q128 0x20ba18
|
|
#define JEDEC_ID_WINBOND_W25Q16 0xEF4015
|
|
#define JEDEC_ID_WINBOND_W25Q32 0xEF4016
|
|
#define JEDEC_ID_WINBOND_W25Q64 0xEF4017
|
|
#define JEDEC_ID_WINBOND_W25Q128 0xEF4018
|
|
#define JEDEC_ID_WINBOND_W25Q256 0xEF4019
|
|
#define JEDEC_ID_CYPRESS_S25FL128L 0x016018
|
|
|
|
void AP_Logger_DataFlash::Init()
|
|
{
|
|
dev = hal.spi->get_device("dataflash");
|
|
if (!dev) {
|
|
AP_HAL::panic("PANIC: AP_Logger SPIDeviceDriver not found");
|
|
return;
|
|
}
|
|
|
|
dev_sem = dev->get_semaphore();
|
|
|
|
if (!getSectorCount()) {
|
|
flash_died = true;
|
|
return;
|
|
}
|
|
|
|
if (use_32bit_address) {
|
|
Enter4ByteAddressMode();
|
|
}
|
|
|
|
flash_died = false;
|
|
|
|
AP_Logger_Block::Init();
|
|
|
|
//flash_test();
|
|
}
|
|
|
|
/*
|
|
wait for busy flag to be cleared
|
|
*/
|
|
void AP_Logger_DataFlash::WaitReady()
|
|
{
|
|
if (flash_died) {
|
|
return;
|
|
}
|
|
|
|
uint32_t t = AP_HAL::millis();
|
|
while (Busy()) {
|
|
hal.scheduler->delay_microseconds(100);
|
|
if (AP_HAL::millis() - t > 5000) {
|
|
printf("DataFlash: flash_died\n");
|
|
flash_died = true;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
bool AP_Logger_DataFlash::getSectorCount(void)
|
|
{
|
|
WaitReady();
|
|
|
|
WITH_SEMAPHORE(dev_sem);
|
|
|
|
// Read manufacturer ID
|
|
uint8_t cmd = JEDEC_DEVICE_ID;
|
|
dev->transfer(&cmd, 1, buffer, 4);
|
|
|
|
uint32_t id = buffer[0] << 16 | buffer[1] << 8 | buffer[2];
|
|
|
|
uint32_t blocks = 0;
|
|
|
|
switch (id) {
|
|
case JEDEC_ID_WINBOND_W25Q16:
|
|
case JEDEC_ID_MICRON_M25P16:
|
|
blocks = 32;
|
|
df_PagePerBlock = 256;
|
|
df_PagePerSector = 16;
|
|
break;
|
|
case JEDEC_ID_WINBOND_W25Q32:
|
|
case JEDEC_ID_MACRONIX_MX25L3206E:
|
|
blocks = 64;
|
|
df_PagePerBlock = 256;
|
|
df_PagePerSector = 16;
|
|
break;
|
|
case JEDEC_ID_MICRON_N25Q064:
|
|
case JEDEC_ID_WINBOND_W25Q64:
|
|
case JEDEC_ID_MACRONIX_MX25L6406E:
|
|
blocks = 128;
|
|
df_PagePerBlock = 256;
|
|
df_PagePerSector = 16;
|
|
break;
|
|
case JEDEC_ID_MICRON_N25Q128:
|
|
case JEDEC_ID_WINBOND_W25Q128:
|
|
case JEDEC_ID_CYPRESS_S25FL128L:
|
|
blocks = 256;
|
|
df_PagePerBlock = 256;
|
|
df_PagePerSector = 16;
|
|
break;
|
|
case JEDEC_ID_WINBOND_W25Q256:
|
|
case JEDEC_ID_MACRONIX_MX25L25635E:
|
|
blocks = 512;
|
|
df_PagePerBlock = 256;
|
|
df_PagePerSector = 16;
|
|
use_32bit_address = true;
|
|
break;
|
|
default:
|
|
hal.scheduler->delay(2000);
|
|
printf("Unknown SPI Flash 0x%08x\n", id);
|
|
return false;
|
|
}
|
|
|
|
df_PageSize = 256;
|
|
df_NumPages = blocks * df_PagePerBlock;
|
|
erase_cmd = JEDEC_BLOCK64_ERASE;
|
|
|
|
printf("SPI Flash 0x%08x found pages=%u erase=%uk\n",
|
|
id, df_NumPages, (df_PagePerBlock * (uint32_t)df_PageSize)/1024);
|
|
return true;
|
|
|
|
}
|
|
|
|
// Read the status register
|
|
uint8_t AP_Logger_DataFlash::ReadStatusReg()
|
|
{
|
|
WITH_SEMAPHORE(dev_sem);
|
|
uint8_t cmd = JEDEC_READ_STATUS;
|
|
uint8_t status;
|
|
dev->transfer(&cmd, 1, &status, 1);
|
|
return status;
|
|
}
|
|
|
|
bool AP_Logger_DataFlash::Busy()
|
|
{
|
|
return (ReadStatusReg() & (JEDEC_STATUS_BUSY | JEDEC_STATUS_SRP0)) != 0;
|
|
}
|
|
|
|
void AP_Logger_DataFlash::Enter4ByteAddressMode(void)
|
|
{
|
|
WITH_SEMAPHORE(dev_sem);
|
|
|
|
const uint8_t cmd = 0xB7;
|
|
dev->transfer(&cmd, 1, NULL, 0);
|
|
}
|
|
|
|
/*
|
|
send a command with an address
|
|
*/
|
|
void AP_Logger_DataFlash::send_command_addr(uint8_t command, uint32_t PageAdr)
|
|
{
|
|
uint8_t cmd[5];
|
|
cmd[0] = command;
|
|
if (use_32bit_address) {
|
|
cmd[1] = (PageAdr >> 24) & 0xff;
|
|
cmd[2] = (PageAdr >> 16) & 0xff;
|
|
cmd[3] = (PageAdr >> 8) & 0xff;
|
|
cmd[4] = (PageAdr >> 0) & 0xff;
|
|
} else {
|
|
cmd[1] = (PageAdr >> 16) & 0xff;
|
|
cmd[2] = (PageAdr >> 8) & 0xff;
|
|
cmd[3] = (PageAdr >> 0) & 0xff;
|
|
}
|
|
|
|
dev->transfer(cmd, use_32bit_address?5:4, NULL, 0);
|
|
}
|
|
|
|
|
|
void AP_Logger_DataFlash::PageToBuffer(uint32_t pageNum)
|
|
{
|
|
if (pageNum == 0 || pageNum > df_NumPages+1) {
|
|
printf("Invalid page read %u\n", pageNum);
|
|
memset(buffer, 0xFF, df_PageSize);
|
|
return;
|
|
}
|
|
WaitReady();
|
|
|
|
uint32_t PageAdr = (pageNum-1) * df_PageSize;
|
|
|
|
WITH_SEMAPHORE(dev_sem);
|
|
dev->set_chip_select(true);
|
|
send_command_addr(JEDEC_READ_DATA, PageAdr);
|
|
dev->transfer(NULL, 0, buffer, df_PageSize);
|
|
dev->set_chip_select(false);
|
|
}
|
|
|
|
void AP_Logger_DataFlash::BufferToPage(uint32_t pageNum)
|
|
{
|
|
if (pageNum == 0 || pageNum > df_NumPages+1) {
|
|
printf("Invalid page write %u\n", pageNum);
|
|
return;
|
|
}
|
|
WriteEnable();
|
|
|
|
WITH_SEMAPHORE(dev_sem);
|
|
|
|
uint32_t PageAdr = (pageNum-1) * df_PageSize;
|
|
|
|
dev->set_chip_select(true);
|
|
send_command_addr(JEDEC_PAGE_WRITE, PageAdr);
|
|
dev->transfer(buffer, df_PageSize, NULL, 0);
|
|
dev->set_chip_select(false);
|
|
}
|
|
|
|
/*
|
|
erase one sector (sizes varies with hw)
|
|
*/
|
|
void AP_Logger_DataFlash::SectorErase(uint32_t blockNum)
|
|
{
|
|
WriteEnable();
|
|
|
|
WITH_SEMAPHORE(dev_sem);
|
|
|
|
uint32_t PageAdr = blockNum * df_PageSize * df_PagePerBlock;
|
|
send_command_addr(erase_cmd, PageAdr);
|
|
}
|
|
|
|
/*
|
|
erase one 4k sector
|
|
*/
|
|
void AP_Logger_DataFlash::Sector4kErase(uint32_t sectorNum)
|
|
{
|
|
WriteEnable();
|
|
|
|
WITH_SEMAPHORE(dev_sem);
|
|
uint32_t SectorAddr = sectorNum * df_PageSize * df_PagePerSector;
|
|
send_command_addr(JEDEC_SECTOR4_ERASE, SectorAddr);
|
|
}
|
|
|
|
void AP_Logger_DataFlash::StartErase()
|
|
{
|
|
WriteEnable();
|
|
|
|
WITH_SEMAPHORE(dev_sem);
|
|
|
|
uint8_t cmd = JEDEC_BULK_ERASE;
|
|
dev->transfer(&cmd, 1, NULL, 0);
|
|
|
|
erase_start_ms = AP_HAL::millis();
|
|
printf("Dataflash: erase started\n");
|
|
}
|
|
|
|
bool AP_Logger_DataFlash::InErase()
|
|
{
|
|
if (erase_start_ms && !Busy()) {
|
|
printf("Dataflash: erase done (%u ms)\n", AP_HAL::millis() - erase_start_ms);
|
|
erase_start_ms = 0;
|
|
}
|
|
return erase_start_ms != 0;
|
|
}
|
|
|
|
void AP_Logger_DataFlash::WriteEnable(void)
|
|
{
|
|
WaitReady();
|
|
WITH_SEMAPHORE(dev_sem);
|
|
uint8_t b = JEDEC_WRITE_ENABLE;
|
|
dev->transfer(&b, 1, NULL, 0);
|
|
}
|
|
|
|
void AP_Logger_DataFlash::flash_test()
|
|
{
|
|
// wait for the chip to be ready, this has been moved from Init()
|
|
hal.scheduler->delay(2000);
|
|
|
|
for (uint8_t i=1; i<=20; i++) {
|
|
printf("Flash fill %u\n", i);
|
|
if (i % df_PagePerBlock == 0) {
|
|
SectorErase(i / df_PagePerBlock);
|
|
}
|
|
memset(buffer, i, df_PageSize);
|
|
BufferToPage(i);
|
|
}
|
|
for (uint8_t i=1; i<=20; i++) {
|
|
printf("Flash check %u\n", i);
|
|
PageToBuffer(i);
|
|
for (uint16_t j=0; j<df_PageSize; j++) {
|
|
if (buffer[j] != i) {
|
|
printf("Test error: page %u j=%u v=%u\n", i, j, buffer[j]);
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
#endif // HAL_LOGGING_DATAFLASH
|