mirror of https://github.com/ArduPilot/ardupilot
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
|