2011-12-26 04:34:06 -04:00
|
|
|
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
|
|
|
|
/*
|
2012-08-21 23:19:52 -03:00
|
|
|
* DataFlash.cpp - DataFlash log library generic code
|
|
|
|
*/
|
2011-12-26 04:34:06 -04:00
|
|
|
|
2012-12-08 00:35:58 -04:00
|
|
|
#include <AP_HAL.h>
|
2011-12-26 04:34:06 -04:00
|
|
|
#include "DataFlash.h"
|
|
|
|
|
2012-12-08 00:35:58 -04:00
|
|
|
extern AP_HAL::HAL& hal;
|
2011-12-26 04:34:06 -04:00
|
|
|
|
2013-05-08 04:00:36 -03:00
|
|
|
// the last page holds the log format in first 4 bytes. Please change
|
|
|
|
// this if (and only if!) the low level format changes
|
|
|
|
#define DF_LOGGING_FORMAT 0x28122013
|
|
|
|
|
2011-12-26 04:34:06 -04:00
|
|
|
// *** DATAFLASH PUBLIC FUNCTIONS ***
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::StartWrite(uint16_t PageAdr)
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2013-01-12 02:21:04 -04:00
|
|
|
df_BufferIdx = 0;
|
|
|
|
df_BufferNum = 0;
|
|
|
|
df_PageAdr = PageAdr;
|
2011-12-26 04:34:06 -04:00
|
|
|
WaitReady();
|
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::FinishWrite(void)
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2013-01-12 02:21:04 -04:00
|
|
|
// Write Buffer to flash, NO WAIT
|
|
|
|
BufferToPage(df_BufferNum, df_PageAdr, 0);
|
2012-08-21 23:19:52 -03:00
|
|
|
df_PageAdr++;
|
2013-01-12 02:21:04 -04:00
|
|
|
// If we reach the end of the memory, start from the begining
|
|
|
|
if (df_PageAdr > df_NumPages)
|
|
|
|
df_PageAdr = 1;
|
2011-12-26 04:34:06 -04:00
|
|
|
|
2013-01-12 02:21:04 -04:00
|
|
|
// switch buffer
|
|
|
|
df_BufferNum ^= 1;
|
|
|
|
df_BufferIdx = 0;
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::WriteBlock(const void *pBuffer, uint16_t size)
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2013-05-14 03:27:27 -03:00
|
|
|
if (!CardInserted() || !log_write_started) {
|
2013-04-01 23:07:56 -03:00
|
|
|
return;
|
|
|
|
}
|
2013-01-12 02:21:04 -04:00
|
|
|
while (size > 0) {
|
|
|
|
uint16_t n = df_PageSize - df_BufferIdx;
|
|
|
|
if (n > size) {
|
|
|
|
n = size;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (df_BufferIdx == 0) {
|
|
|
|
// if we are at the start of a page we need to insert a
|
|
|
|
// page header
|
|
|
|
if (n > df_PageSize - sizeof(struct PageHeader)) {
|
2013-07-22 20:46:02 -03:00
|
|
|
n = df_PageSize - sizeof(struct PageHeader);
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
2013-01-12 02:21:04 -04:00
|
|
|
struct PageHeader ph = { df_FileNumber, df_FilePage };
|
|
|
|
BlockWrite(df_BufferNum, df_BufferIdx, &ph, sizeof(ph), pBuffer, n);
|
|
|
|
df_BufferIdx += n + sizeof(ph);
|
|
|
|
} else {
|
|
|
|
BlockWrite(df_BufferNum, df_BufferIdx, NULL, 0, pBuffer, n);
|
|
|
|
df_BufferIdx += n;
|
|
|
|
}
|
2012-08-15 18:26:09 -03:00
|
|
|
|
2013-01-12 02:21:04 -04:00
|
|
|
size -= n;
|
|
|
|
pBuffer = (const void *)(n + (uintptr_t)pBuffer);
|
2011-12-26 04:34:06 -04:00
|
|
|
|
2013-01-12 02:21:04 -04:00
|
|
|
if (df_BufferIdx == df_PageSize) {
|
|
|
|
FinishWrite();
|
2011-12-26 04:34:06 -04:00
|
|
|
df_FilePage++;
|
|
|
|
}
|
|
|
|
}
|
|
|
|
}
|
|
|
|
|
2013-01-12 02:21:04 -04:00
|
|
|
|
2011-12-26 04:34:06 -04:00
|
|
|
// Get the last page written to
|
2013-02-23 03:52:30 -04:00
|
|
|
uint16_t DataFlash_Block::GetWritePage()
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
|
|
|
return df_PageAdr;
|
|
|
|
}
|
|
|
|
|
|
|
|
// Get the last page read
|
2013-02-23 03:52:30 -04:00
|
|
|
uint16_t DataFlash_Block::GetPage()
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2013-01-14 23:02:08 -04:00
|
|
|
return df_Read_PageAdr;
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::StartRead(uint16_t PageAdr)
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2013-01-12 02:21:04 -04:00
|
|
|
df_Read_BufferNum = 0;
|
|
|
|
df_Read_PageAdr = PageAdr;
|
|
|
|
|
2013-12-15 03:57:49 -04:00
|
|
|
// disable writing while reading
|
|
|
|
log_write_started = false;
|
|
|
|
|
2011-12-26 04:34:06 -04:00
|
|
|
WaitReady();
|
2013-01-12 02:21:04 -04:00
|
|
|
|
|
|
|
// copy flash page to buffer
|
|
|
|
PageToBuffer(df_Read_BufferNum, df_Read_PageAdr);
|
2011-12-26 04:34:06 -04:00
|
|
|
|
|
|
|
// We are starting a new page - read FileNumber and FilePage
|
2013-01-12 02:21:04 -04:00
|
|
|
struct PageHeader ph;
|
|
|
|
BlockRead(df_Read_BufferNum, 0, &ph, sizeof(ph));
|
|
|
|
df_FileNumber = ph.FileNumber;
|
|
|
|
df_FilePage = ph.FilePage;
|
|
|
|
df_Read_BufferIdx = sizeof(ph);
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::ReadBlock(void *pBuffer, uint16_t size)
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2013-01-12 02:21:04 -04:00
|
|
|
while (size > 0) {
|
|
|
|
uint16_t n = df_PageSize - df_Read_BufferIdx;
|
|
|
|
if (n > size) {
|
|
|
|
n = size;
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
2012-08-15 18:26:09 -03:00
|
|
|
|
2013-01-12 02:21:04 -04:00
|
|
|
WaitReady();
|
|
|
|
|
|
|
|
BlockRead(df_Read_BufferNum, df_Read_BufferIdx, pBuffer, n);
|
|
|
|
size -= n;
|
|
|
|
pBuffer = (void *)(n + (uintptr_t)pBuffer);
|
|
|
|
|
|
|
|
df_Read_BufferIdx += n;
|
|
|
|
|
|
|
|
if (df_Read_BufferIdx == df_PageSize) {
|
|
|
|
df_Read_PageAdr++;
|
|
|
|
if (df_Read_PageAdr > df_NumPages) {
|
|
|
|
df_Read_PageAdr = 1;
|
|
|
|
}
|
|
|
|
PageToBuffer(df_Read_BufferNum, df_Read_PageAdr);
|
|
|
|
|
|
|
|
// We are starting a new page - read FileNumber and FilePage
|
|
|
|
struct PageHeader ph;
|
|
|
|
BlockRead(df_Read_BufferNum, 0, &ph, sizeof(ph));
|
|
|
|
df_FileNumber = ph.FileNumber;
|
|
|
|
df_FilePage = ph.FilePage;
|
|
|
|
|
|
|
|
df_Read_BufferIdx = sizeof(ph);
|
|
|
|
}
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
2013-01-12 02:21:04 -04:00
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::SetFileNumber(uint16_t FileNumber)
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2012-08-21 23:19:52 -03:00
|
|
|
df_FileNumber = FileNumber;
|
|
|
|
df_FilePage = 1;
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
uint16_t DataFlash_Block::GetFileNumber()
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2012-08-21 23:19:52 -03:00
|
|
|
return df_FileNumber;
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
uint16_t DataFlash_Block::GetFilePage()
|
2011-12-26 04:34:06 -04:00
|
|
|
{
|
2012-08-21 23:19:52 -03:00
|
|
|
return df_FilePage;
|
2011-12-26 04:34:06 -04:00
|
|
|
}
|
2011-12-28 00:52:36 -04:00
|
|
|
|
2013-02-23 03:52:30 -04:00
|
|
|
void DataFlash_Block::EraseAll()
|
2011-12-28 00:52:36 -04:00
|
|
|
{
|
2013-05-08 04:00:36 -03:00
|
|
|
for (uint16_t j = 1; j <= (df_NumPages+1)/8; j++) {
|
2012-08-21 23:19:52 -03:00
|
|
|
BlockErase(j);
|
2012-12-08 00:35:58 -04:00
|
|
|
if (j%6 == 0) {
|
|
|
|
hal.scheduler->delay(6);
|
|
|
|
}
|
2012-08-21 23:19:52 -03:00
|
|
|
}
|
2011-12-28 00:52:36 -04:00
|
|
|
// write the logging format in the last page
|
2013-05-08 04:00:36 -03:00
|
|
|
hal.scheduler->delay(100);
|
2011-12-28 00:52:36 -04:00
|
|
|
StartWrite(df_NumPages+1);
|
2013-02-22 19:13:59 -04:00
|
|
|
uint32_t version = DF_LOGGING_FORMAT;
|
2013-05-14 03:27:27 -03:00
|
|
|
log_write_started = true;
|
2013-02-22 19:13:59 -04:00
|
|
|
WriteBlock(&version, sizeof(version));
|
2013-05-14 03:27:27 -03:00
|
|
|
log_write_started = false;
|
2012-08-21 23:19:52 -03:00
|
|
|
FinishWrite();
|
2013-05-08 04:00:36 -03:00
|
|
|
hal.scheduler->delay(100);
|
2011-12-28 00:52:36 -04:00
|
|
|
}
|
|
|
|
|
|
|
|
/*
|
2012-08-21 23:19:52 -03:00
|
|
|
* we need to erase if the logging format has changed
|
2011-12-28 00:52:36 -04:00
|
|
|
*/
|
2013-02-23 03:52:30 -04:00
|
|
|
bool DataFlash_Block::NeedErase(void)
|
2011-12-28 00:52:36 -04:00
|
|
|
{
|
2013-05-14 03:27:27 -03:00
|
|
|
uint32_t version = 0;
|
2011-12-28 00:52:36 -04:00
|
|
|
StartRead(df_NumPages+1);
|
2013-02-22 19:13:59 -04:00
|
|
|
ReadBlock(&version, sizeof(version));
|
2013-05-14 03:27:27 -03:00
|
|
|
StartRead(1);
|
2013-02-22 19:13:59 -04:00
|
|
|
return version != DF_LOGGING_FORMAT;
|
2011-12-28 00:52:36 -04:00
|
|
|
}
|
2013-12-15 03:57:49 -04:00
|
|
|
|
2013-12-16 20:14:33 -04:00
|
|
|
/**
|
|
|
|
get raw data from a log
|
2013-12-15 03:57:49 -04:00
|
|
|
*/
|
2013-12-16 20:14:33 -04:00
|
|
|
int16_t DataFlash_Block::get_log_data_raw(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
|
2013-12-15 03:57:49 -04:00
|
|
|
{
|
|
|
|
uint16_t data_page_size = df_PageSize - sizeof(struct PageHeader);
|
2013-12-16 20:14:33 -04:00
|
|
|
|
2013-12-15 03:57:49 -04:00
|
|
|
if (offset >= data_page_size) {
|
|
|
|
page += offset / data_page_size;
|
|
|
|
offset = offset % data_page_size;
|
2013-12-16 20:14:33 -04:00
|
|
|
if (page > df_NumPages) {
|
|
|
|
// pages are one based, not zero
|
|
|
|
page = 1 + page - df_NumPages;
|
|
|
|
}
|
2013-12-15 03:57:49 -04:00
|
|
|
}
|
|
|
|
if (log_write_started || df_Read_PageAdr != page) {
|
|
|
|
StartRead(page);
|
|
|
|
}
|
|
|
|
|
|
|
|
df_Read_BufferIdx = offset + sizeof(struct PageHeader);
|
|
|
|
ReadBlock(data, len);
|
2013-12-16 20:14:33 -04:00
|
|
|
|
2013-12-15 03:57:49 -04:00
|
|
|
return (int16_t)len;
|
|
|
|
}
|
|
|
|
|
2013-12-16 20:14:33 -04:00
|
|
|
/**
|
|
|
|
get data from a log, accounting for adding FMT headers
|
|
|
|
*/
|
|
|
|
int16_t DataFlash_Block::get_log_data(uint16_t log_num, uint16_t page, uint32_t offset, uint16_t len, uint8_t *data)
|
|
|
|
{
|
|
|
|
if (offset == 0) {
|
|
|
|
uint8_t header[3];
|
|
|
|
get_log_data_raw(log_num, page, 0, 3, header);
|
|
|
|
adding_fmt_headers = (header[0] != HEAD_BYTE1 || header[1] != HEAD_BYTE2 || header[2] != LOG_FORMAT_MSG);
|
|
|
|
}
|
|
|
|
uint16_t ret = 0;
|
|
|
|
|
|
|
|
if (adding_fmt_headers) {
|
|
|
|
// the log doesn't start with a FMT message, we need to add
|
|
|
|
// them
|
|
|
|
const uint16_t fmt_header_size = _num_types * sizeof(struct log_Format);
|
|
|
|
while (offset < fmt_header_size && len > 0) {
|
|
|
|
struct log_Format pkt;
|
|
|
|
uint8_t t = offset / sizeof(pkt);
|
|
|
|
uint8_t ofs = offset % sizeof(pkt);
|
|
|
|
Log_Fill_Format(&_structures[t], pkt);
|
|
|
|
uint8_t n = sizeof(pkt) - ofs;
|
|
|
|
if (n > len) {
|
|
|
|
n = len;
|
|
|
|
}
|
|
|
|
memcpy(data, ofs + (uint8_t *)&pkt, n);
|
|
|
|
data += n;
|
|
|
|
offset += n;
|
|
|
|
len -= n;
|
|
|
|
ret += n;
|
|
|
|
}
|
|
|
|
offset -= fmt_header_size;
|
|
|
|
}
|
|
|
|
|
|
|
|
if (len > 0) {
|
|
|
|
ret += get_log_data_raw(log_num, page, offset, len, data);
|
|
|
|
}
|
|
|
|
|
|
|
|
return ret;
|
|
|
|
}
|