From 474b536484ec07797f0044485b0b20d8306edb19 Mon Sep 17 00:00:00 2001 From: qqqlab <46283638+qqqlab@users.noreply.github.com> Date: Tue, 22 Oct 2024 16:33:57 +0200 Subject: [PATCH] Add ArduPilot logging --- .../02.QuadcopterAdvanced.ino | 6 +- examples/03.PID-Tune/03.PID-Tune.ino | 6 +- examples/04.Plane/04.Plane.ino | 6 +- extras/TestMadflight/bb.ino | 112 -- src/madflight.h | 2 +- src/madflight/bb/BlackBoxDecoder.h | 330 ----- src/madflight/bb/BlackBoxWriter.h | 232 ---- src/madflight/bb/BlackBox_Defines.h | 64 - .../bb/SPIFlash/Adafruit_FlashTransport.h | 147 --- .../Adafruit_FlashTransport_ESP32.cpp | 139 --- .../SPIFlash/Adafruit_FlashTransport_ESP32.h | 60 - .../SPIFlash/Adafruit_FlashTransport_QSPI.h | 54 - .../Adafruit_FlashTransport_QSPI_NRF.cpp | 267 ---- .../Adafruit_FlashTransport_QSPI_SAMD.cpp | 235 ---- .../Adafruit_FlashTransport_RP2040.cpp | 218 ---- .../SPIFlash/Adafruit_FlashTransport_RP2040.h | 90 -- .../SPIFlash/Adafruit_FlashTransport_SPI.cpp | 181 --- .../bb/SPIFlash/Adafruit_FlashTransport_SPI.h | 73 -- .../bb/SPIFlash/Adafruit_SPIFlashBase.cpp | 517 -------- .../bb/SPIFlash/Adafruit_SPIFlashBase.h | 96 -- src/madflight/bb/SPIFlash/flash_devices.h | 542 -------- src/madflight/bb/bb.h | 1089 ++++++++++------- src/madflight/cli/cli.h | 6 +- 23 files changed, 626 insertions(+), 3846 deletions(-) delete mode 100644 extras/TestMadflight/bb.ino delete mode 100644 src/madflight/bb/BlackBoxDecoder.h delete mode 100644 src/madflight/bb/BlackBoxWriter.h delete mode 100644 src/madflight/bb/BlackBox_Defines.h delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport.h delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.cpp delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.h delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI.h delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_NRF.cpp delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_SAMD.cpp delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.cpp delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.h delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.cpp delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.h delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.cpp delete mode 100644 src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.h delete mode 100644 src/madflight/bb/SPIFlash/flash_devices.h diff --git a/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino b/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino index 8330ab9c..75148ab6 100644 --- a/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino +++ b/examples/02.QuadcopterAdvanced/02.QuadcopterAdvanced.ino @@ -340,8 +340,8 @@ void loop() { //update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU void i2c_sensors_update() { - if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. - if(baro.update()) bb.log_baro(); //log if pressure updated + if(bat.update()) bb_log_bat(); //update battery, and log if battery was updated. + if(baro.update()) bb_log_baro(); //log if pressure updated mag.update(); } @@ -383,7 +383,7 @@ void imu_loop() { //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. if (imu.usesI2C()) i2c_sensors_update(); - //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... + //bb_log_imu(); //full speed black box logging of IMU data, memory fills up quickly... } //========================================================================================================================// diff --git a/examples/03.PID-Tune/03.PID-Tune.ino b/examples/03.PID-Tune/03.PID-Tune.ino index 8d39acd8..38e3ea08 100644 --- a/examples/03.PID-Tune/03.PID-Tune.ino +++ b/examples/03.PID-Tune/03.PID-Tune.ino @@ -340,8 +340,8 @@ void loop() { //update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU void i2c_sensors_update() { - if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. - if(baro.update()) bb.log_baro(); //log if pressure updated + if(bat.update()) bb_log_bat(); //update battery, and log if battery was updated. + if(baro.update()) bb_log_baro(); //log if pressure updated mag.update(); } @@ -378,7 +378,7 @@ void imu_loop() { //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. if (imu.usesI2C()) i2c_sensors_update(); - //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... + //bb_log_imu(); //full speed black box logging of IMU data, memory fills up quickly... } //========================================================================================================================// diff --git a/examples/04.Plane/04.Plane.ino b/examples/04.Plane/04.Plane.ino index 7e8893a5..50ae2ccd 100644 --- a/examples/04.Plane/04.Plane.ino +++ b/examples/04.Plane/04.Plane.ino @@ -388,8 +388,8 @@ void loop() { //update all I2C sensors, called from loop() with SPI IMU, or called from imu_loop() with I2C IMU void i2c_sensors_update() { - if(bat.update()) bb.log_bat(); //update battery, and log if battery was updated. - if(baro.update()) bb.log_baro(); //log if pressure updated + if(bat.update()) bb_log_bat(); //update battery, and log if battery was updated. + if(baro.update()) bb_log_baro(); //log if pressure updated mag.update(); } @@ -434,7 +434,7 @@ void imu_loop() { //if IMU uses I2C bus, then get I2C sensor readings in imu_interrupt_handler() to prevent I2C bus collisions. Alternatively, put the IMU on a separate I2C bus. if (imu.usesI2C()) i2c_sensors_update(); - //bb.log_imu(); //full speed black box logging of IMU data, memory fills up quickly... + //bb_log_imu(); //full speed black box logging of IMU data, memory fills up quickly... } //========================================================================================================================// diff --git a/extras/TestMadflight/bb.ino b/extras/TestMadflight/bb.ino deleted file mode 100644 index 851b1f8c..00000000 --- a/extras/TestMadflight/bb.ino +++ /dev/null @@ -1,112 +0,0 @@ -#include "BlackBoxWriter.h" -#include "BlackBoxDecoder.h" - -BlackBox bb; - -enum { - BB_REC_PID, - BB_REC_GPS -}; - -int bb_logCharSerial_cnt = 0; -void bb_logCharSerial(uint8_t c) { - Serial.printf("%02X'%c' ",c,(c>=32&&c<128 ? c : '.')); - bb_logCharSerial_cnt++; - if(bb_logCharSerial_cnt==16) { - Serial.println(); - bb_logCharSerial_cnt=0; - } -} - -const int bb_buf_size = 60000; -uint8_t bb_buf[bb_buf_size]; -int bb_buf_len = 0; -int bb_buf_idx = 0; - -void bb_logCharMemory(uint8_t c) { - if(bb_buf_len < bb_buf_size) { - bb_buf[bb_buf_len++] = c; - } -} - -uint8_t bb_decode_getCharMemory() { - if(bb_buf_idx < bb_buf_len) { - return bb_buf[bb_buf_idx++]; - }else{ - return 0xff; - } -} - -void bb_decode_putChar(uint8_t c) { - Serial.printf("%c",c); -} - -void bb_imu() { - bb.writeBeginRecord(BB_REC_PID); - bb.writeU("ts",11); - bb.writeI("ax",12); - bb.writeI("ay",13); - bb.writeI("az",14); - bb.writeEndrecord(); -} - -void bb_gps() { - bb.writeBeginRecord(BB_REC_GPS); - bb.writeI("ts",-21); - bb.writeI("lat",-2222); - bb.writeFloat("lon",1.23e-2); - bb.writeEndrecord(); -} - -void bb_setup() { - bb.begin(bb_logCharMemory); - bb_imu(); - bb_gps(); - bb.start(); -} - -void setup() { - Serial.begin(115200); - - -} - -void loop() { - Serial.println(); - Serial.println("----------------------------"); - - bb_logCharSerial_cnt = 0; - bb_buf_len = 0; - - bb_setup(); - - for(int i=0;i<5;i++) { - bb_imu(); - bb_imu(); - bb_gps(); - } - - int ii = 0; - for(int i=0;i=32&&c<128 ? c : '.')); - } - Serial.println(); - ii = i+1; - } - } - - Serial.println(); - Serial.printf("bb_buf_len=%d\n",bb_buf_len); - - bb_buf_idx = 0; - BlackBoxDecoder bbd; - bbd.csv_decode(bb_decode_getCharMemory, bb_decode_putChar); - - delay(1000); -} \ No newline at end of file diff --git a/src/madflight.h b/src/madflight.h index 41409e88..37c3e929 100644 --- a/src/madflight.h +++ b/src/madflight.h @@ -1,4 +1,4 @@ -#define MADFLIGHT_VERSION "madflight v1.2.1-DEV" +#define MADFLIGHT_VERSION "madflight v1.3.0-DEV" /*========================================================================================== madflight - Flight Controller for ESP32 / RP2040 / STM32 diff --git a/src/madflight/bb/BlackBoxDecoder.h b/src/madflight/bb/BlackBoxDecoder.h deleted file mode 100644 index 7a7da3d7..00000000 --- a/src/madflight/bb/BlackBoxDecoder.h +++ /dev/null @@ -1,330 +0,0 @@ -/*============================================================================================== -Black Box Decoder -==============================================================================================*/ - -#pragma once - -#include "BlackBox_Defines.h" - -class BlackBoxDecoder { -public: - - bool addRecNoColumn = true; //add record number column - char separator = ','; //column separator - - //Note: this function expects 0xff from getChar() when fetching past the end of buffer - void csv_decode(BlackBoxFS *fs) { - this->fs = fs; - - //read file start marker - for(int i=0;i<16;i++){ - uint8_t c = fs->readChar(); - if(c != BB_STARTLOG[i]) { - Serial.println("Invalid BlackBox file"); - return; - } - } - - recNo = 0; - sl_cnt = 0; - ff_cnt = 0; - - bool lastCharWasEndRecord = true; - bool csvHeaderWritten = false; - while(true) { - if(eof) return; //exit on eof - - uint8_t c = readChar(); - - if(lastCharWasEndRecord) { - //look for record type byte - if(c == BB_HDR_REC_TYPE) { - //got a header - lastCharWasEndRecord = parseHeader(); - }else if(creadChar(); - //detect end of stream - if(c == 0xff) { - ff_cnt++; - if(ff_cnt > BB_MAX_REC_CHARS) { - eof = true; - ff_cnt = 0; - } - }else{ - ff_cnt = 0; - } - - //detect start of next file - if(c == BB_STARTLOG[sl_cnt]) { - sl_cnt++; - if(sl_cnt==16) { - eof = true; - sl_cnt = 0; - } - }else{ - sl_cnt=0; - } - - return c; - } - - void _putChar(uint8_t c) { - Serial.printf("%c",c); - } - - struct Field_s { - uint8_t column = 0xff; - bb_datatype_e datatype = BB_DATATYPE_COUNT; - } fields[BB_REC_TYPE_SIZE][BB_MAX_FIELD_COUNT]; - - bool parseHeader() { - uint8_t hdrType = readChar(); - if (hdrType == BB_HDRTYPE_REC) { //BB_HDRTYPE_REC: recordtype, name - //recordtype - uint8_t recordtype = readChar(); - if(recordtype >= BB_REC_TYPE_SIZE) return false; - //name - String name = ""; - uint8_t c; - for(int i=0;i= BB_REC_TYPE_SIZE) return false; - //fieldindex - uint8_t fieldindex = readChar(); - if(fieldindex >= BB_MAX_FIELD_COUNT) return false; - //datatype - uint8_t datatype = readChar(); - if(datatype >= BB_DATATYPE_COUNT) return false; - //name - String name = ""; - uint8_t c; - for(int i=0;i>1)-1 : (value>>1) ); - } - -}; diff --git a/src/madflight/bb/BlackBoxWriter.h b/src/madflight/bb/BlackBoxWriter.h deleted file mode 100644 index f31a18c6..00000000 --- a/src/madflight/bb/BlackBoxWriter.h +++ /dev/null @@ -1,232 +0,0 @@ -/*============================================================================================== -Black Box Data Logger -==============================================================================================*/ - -#pragma once - -#include "BlackBox_Defines.h" - -class BlackBoxWriter { - -private: - -BlackBoxFS *fs; - -uint8_t logStarted = false; //enable logging -bool busy = false; - -//vars for headers -uint8_t startWritten = false; -uint8_t writingHeaders = false; -uint8_t currentRecType; -uint8_t currentFieldIndex; -bool recTypeUsed[BB_REC_TYPE_SIZE]; - -public: - -void setup( BlackBoxFS *fs ) { - this->fs = fs; - - logStarted = false; - startWritten = false; - writingHeaders = false; - busy = false; - currentRecType = BB_REC_TYPE_SIZE; - currentFieldIndex = 0; - for(int i=0;iwriteChar(type); - }else{ - writeHeaderRec(type, name); - } -} - -void writeFloat(String name, float value) { - if(logStarted) { - logFloat(value); - }else{ - writeHeaderField(name, BB_DATATYPE_FLOAT); - } -} - -void writeU32(String name, uint32_t value) { - if(logStarted) { - logU32(value); - }else{ - writeHeaderField(name, BB_DATATYPE_U32); - } -} - -void writeI32(String name, int32_t value) { - if(logStarted) { - logU32((uint32_t)value); - }else{ - writeHeaderField(name, BB_DATATYPE_I32); - } -} - -void writeU16(String name, uint16_t value) { - if(logStarted) { - logU16(value); - }else{ - writeHeaderField(name, BB_DATATYPE_U16); - } -} - -void writeI16(String name, int16_t value) { - if(logStarted) { - logU16((uint32_t)value); - }else{ - writeHeaderField(name, BB_DATATYPE_I16); - } -} - -void writeU(String name, uint32_t value) { - if(logStarted) { - logU(value); - }else{ - writeHeaderField(name, BB_DATATYPE_UVB); - } -} - -void writeI(String name, int32_t value) { - if(logStarted) { - logI((uint32_t)value); - }else{ - writeHeaderField(name, BB_DATATYPE_SVB); - } -} - -void writeEndrecord() { - busy = false; - if(logStarted) { - fs->writeChar(BB_ENDRECORD); - }else if(currentRecTypewriteChar(BB_HDR_REC_TYPE); - fs->writeChar(BB_HDRTYPE_REC); - fs->writeChar(type); - logString(name); - fs->writeChar(BB_ENDRECORD); - currentRecType = type; - currentFieldIndex = 0; - }else { - currentRecType = BB_REC_TYPE_SIZE; - } -} - -void writeHeaderField(String name, bb_datatype_e datatype) { - if(writingHeaders && currentRecTypewriteChar(BB_HDR_REC_TYPE); - fs->writeChar(BB_HDRTYPE_FIELD); - fs->writeChar(currentRecType); - fs->writeChar(currentFieldIndex); - fs->writeChar(datatype); - logString(name); - fs->writeChar(BB_ENDRECORD); - currentFieldIndex++; - } -} - - -//------------------------------------------------- -//log to output device functions -//------------------------------------------------- -void logString(String s) { - for(uint32_t i=0;iwriteChar(s[i]); -} - -void logU32(uint32_t value) { - uint8_t *buf = (uint8_t*)&value; - fs->writeChar(buf[0]); - fs->writeChar(buf[1]); - fs->writeChar(buf[2]); - fs->writeChar(buf[3]); -} - -void logU16(uint16_t value) { - uint8_t *buf = (uint8_t*)&value; - fs->writeChar(buf[0]); - fs->writeChar(buf[1]); -} - -void logFloat(float value) { - uint8_t *buf = (uint8_t*)&value; - fs->writeChar(buf[0]); - fs->writeChar(buf[1]); - fs->writeChar(buf[2]); - fs->writeChar(buf[3]); -} - -//Write an unsigned integer to the blackbox using variable byte encoding. -void logU(uint32_t value) -{ - //While this isn't the final byte (we can only write 7 bits at a time) - while (value > 127) { - fs->writeChar((uint8_t) (value | 0x80)); // Set the high bit to mean "more bytes follow" - value >>= 7; - } - fs->writeChar(value); -} - - -//Write a signed integer to the blackbox using ZigZig and variable byte encoding. -void logI(int32_t value) -{ - //ZigZag encode to make the value always positive - logU(zigzagEncode(value)); -} - -/** - * ZigZag encoding maps all values of a signed integer into those of an unsigned integer in such - * a way that numbers of small absolute value correspond to small integers in the result. - * - * (Compared to just casting a signed to an unsigned which creates huge resulting numbers for - * small negative integers). - */ -uint32_t zigzagEncode(int32_t value) -{ - return (uint32_t)((value << 1) ^ (value >> 31)); -} - - -}; \ No newline at end of file diff --git a/src/madflight/bb/BlackBox_Defines.h b/src/madflight/bb/BlackBox_Defines.h deleted file mode 100644 index 392f62bf..00000000 --- a/src/madflight/bb/BlackBox_Defines.h +++ /dev/null @@ -1,64 +0,0 @@ -/*============================================================================================== -Header Record: BB_HDR_REC_TYPE BB_HDRTYPE_xxx data[] BB_ENDRECORD -Record Name: BB_HDR_REC_TYPE BB_HDRTYPE_REC recordType name[] BB_ENDRECORD -Field Definition: BB_HDR_REC_TYPE BB_HDRTYPE_FIELD recordType fieldIndex dataType name[] BB_ENDRECORD -Data Record: recordType data[] BB_ENDRECORD - -example file: - -qqqlab/BlackBox1 -- file header -32 R P PID \n -- record: type='A' name="ACCEL" -32 F P 0 U ts \n -- field: type='A' index=0 datatype=uint32_t name="ts" -32 F P 1 F ax \n -- field: type='A' index=1 datatype=float name="ax" -32 F P 2 F ay \n -- field: type='A' index=2 datatype=float name="ay" -32 F P 3 F az \n -- field: type='A' index=3 datatype=float name="az" -32 R G GPS \n -- record: type='G' name="GPS" -32 F G 0 U ts \n -- field: type='G' index=0 datatype=uint32_t name="ts" -32 F G 1 I lat \n -- field: type='G' index=0 datatype=uint32_t name="lat" -32 F G 2 I lon \n -- field: type='G' index=0 datatype=uint32_t name="lon" -A \n -- ACCEL data: type='A' values=ts,ax,ay,az -A \n -- ACCEL data: type='A' values=ts,ax,ay,az -G \n -- GPS data: type='G' values=ts,lat,lon -A \n -- ACCEL data: type='A' values=ts,ax,ay,az -A \n -- ACCEL data: type='A' values=ts,ax,ay,az -A \n -- ACCEL data: type='A' values=ts,ax,ay,az -G \n -- GPS data: type='G' values=ts,lat,lon -==============================================================================================*/ - -#pragma once - -#define BB_STARTLOG "qqqlabBlackBox1\n" //16 character log start marker -#define BB_REC_TYPE_SIZE 32 //max number of record types (plus one for BB_HDR_REC_TYPE) -#define BB_MAX_REC_CHARS 64 //maximum number characters in a record -#define BB_MAX_FIELD_COUNT 16 //maximum number of fields in a record -#define BB_MAX_COLUMN_COUNT 32 //maximum number of unique column names -#define BB_HDR_REC_TYPE BB_REC_TYPE_SIZE //record type for headers -#define BB_HDRTYPE_FIELD 'F' -#define BB_HDRTYPE_REC 'R' -#define BB_ENDRECORD '\n' //end record character - -enum bb_datatype_e { - BB_DATATYPE_FLOAT, //4 bytes - BB_DATATYPE_U32, //4 bytes - BB_DATATYPE_I32, //4 bytes - BB_DATATYPE_U16, //2 bytes - BB_DATATYPE_I16, //2 bytes - BB_DATATYPE_UVB, //1-5 bytes - BB_DATATYPE_SVB, //1-5 bytes - BB_DATATYPE_COUNT -}; - -class BlackBoxFS { -public: - virtual void setup() = 0; //setup the file system - - virtual void writeOpen() = 0; //create new file for writing - virtual void writeChar(uint8_t c) = 0; //write char to file - virtual void writeClose() = 0; //close file - - virtual void readOpen(int fileno); //open fileno for reading. Fileno 0:last file, -1:2nd last file, etc - virtual uint8_t readChar() = 0; //read char from file - - virtual void erase() = 0; //erase all files - virtual void dir() = 0; //list files -}; \ No newline at end of file diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport.h b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport.h deleted file mode 100644 index 02ac5e66..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport.h +++ /dev/null @@ -1,147 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2019 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef ADAFRUIT_FLASHTRANSPORT_H_ -#define ADAFRUIT_FLASHTRANSPORT_H_ - -#include -#include - -enum { - SFLASH_CMD_READ = 0x03, // Single Read - SFLASH_CMD_FAST_READ = 0x0B, // Fast Read - SFLASH_CMD_QUAD_READ = 0x6B, // 1 line address, 4 line data - - SFLASH_CMD_READ_JEDEC_ID = 0x9f, - - SFLASH_CMD_PAGE_PROGRAM = 0x02, - SFLASH_CMD_QUAD_PAGE_PROGRAM = 0x32, // 1 line address, 4 line data - - SFLASH_CMD_READ_STATUS = 0x05, - SFLASH_CMD_READ_STATUS2 = 0x35, - - SFLASH_CMD_WRITE_STATUS = 0x01, - SFLASH_CMD_WRITE_STATUS2 = 0x31, - - SFLASH_CMD_ENABLE_RESET = 0x66, - SFLASH_CMD_RESET = 0x99, - - SFLASH_CMD_WRITE_ENABLE = 0x06, - SFLASH_CMD_WRITE_DISABLE = 0x04, - - SFLASH_CMD_ERASE_PAGE = 0x81, - SFLASH_CMD_ERASE_SECTOR = 0x20, - SFLASH_CMD_ERASE_BLOCK = 0xD8, - SFLASH_CMD_ERASE_CHIP = 0xC7, - - SFLASH_CMD_4_BYTE_ADDR = 0xB7, - SFLASH_CMD_3_BYTE_ADDR = 0xE9, -}; - -/// Constant that is (mostly) true to all external flash devices -enum { - SFLASH_BLOCK_SIZE = 64 * 1024UL, - SFLASH_SECTOR_SIZE = 4 * 1024, - SFLASH_PAGE_SIZE = 256, -}; - -class Adafruit_FlashTransport { -public: - virtual void begin(void) = 0; - virtual void end(void) = 0; - - virtual bool supportQuadMode(void) = 0; - - /// Set clock speed in hertz - /// @param write_hz Write clock speed in hertz - /// @param read_hz Read clock speed in hertz - virtual void setClockSpeed(uint32_t write_hz, uint32_t read_hz) = 0; - - /// Execute a single byte command e.g Reset, Write Enable - /// @param command command code - /// @return true if success - virtual bool runCommand(uint8_t command) = 0; - - /// Execute a command with response data e.g Read Status, Read JEDEC - /// @param command command code - /// @param response buffer to hold data - /// @param len number of bytes to read - /// @return true if success - virtual bool readCommand(uint8_t command, uint8_t *response, - uint32_t len) = 0; - - /// Execute a command with data e.g Write Status, - /// @param command command code - /// @param data writing data - /// @param len number of bytes to read - /// @return true if success - virtual bool writeCommand(uint8_t command, uint8_t const *data, - uint32_t len) = 0; - - /// Erase external flash by address - /// @param command can be sector erase (0x20) or block erase 0xD8 - /// @param address address to be erased - /// @return true if success - virtual bool eraseCommand(uint8_t command, uint32_t address) = 0; - - /// Read data from external flash contents. Typically it is implemented by - /// quad read command 0x6B - /// @param addr address to read - /// @param buffer buffer to hold data - /// @param len number of byte to read - /// @return true if success - virtual bool readMemory(uint32_t addr, uint8_t *buffer, uint32_t len) = 0; - - /// Write data to external flash contents, flash sector must be previously - /// erased first. Typically it uses quad write command 0x32 - /// @param addr address to read - /// @param data writing data - /// @param len number of byte to read - /// @return true if success - virtual bool writeMemory(uint32_t addr, uint8_t const *data, - uint32_t len) = 0; - - void setAddressLength(uint8_t addr_len) { _addr_len = addr_len; } - void setReadCommand(uint8_t cmd_read) { _cmd_read = cmd_read; } - -protected: - // Number of bytes for address - uint8_t _addr_len; - - // Command use for read operation - uint8_t _cmd_read; -}; - -#include "Adafruit_FlashTransport_QSPI.h" -#include "Adafruit_FlashTransport_SPI.h" - -#ifdef ARDUINO_ARCH_ESP32 -#include "Adafruit_FlashTransport_ESP32.h" -#endif - -#ifdef ARDUINO_ARCH_RP2040 -#include "Adafruit_FlashTransport_RP2040.h" -#endif - -#endif /* ADAFRUIT_FLASHTRANSPORT_H_ */ diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.cpp b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.cpp deleted file mode 100644 index 171e4490..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.cpp +++ /dev/null @@ -1,139 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2020 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "Adafruit_FlashTransport.h" - -#ifdef ARDUINO_ARCH_ESP32 - -#include "esp_flash.h" - -Adafruit_FlashTransport_ESP32::Adafruit_FlashTransport_ESP32(void) { - _cmd_read = SFLASH_CMD_READ; - _addr_len = 3; // work with most device if not set - - _partition = NULL; - memset(&_flash_device, 0, sizeof(_flash_device)); -} - -void Adafruit_FlashTransport_ESP32::begin(void) { - _partition = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_DATA_FAT, NULL); - if(_partition) return; - _partition = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_DATA_SPIFFS, NULL); - //if(_partition) return; - //_partition = esp_partition_find_first(ESP_PARTITION_TYPE_DATA, ESP_PARTITION_SUBTYPE_DATA_LITTLEFS, NULL); - if (!_partition) { - log_printf("SPIFlash: No FAT/SPIFFS partition found"); - } -} - -void Adafruit_FlashTransport_ESP32::end(void) { - _cmd_read = SFLASH_CMD_READ; - _addr_len = 3; // work with most device if not set - - _partition = NULL; - memset(&_flash_device, 0, sizeof(_flash_device)); -} - -SPIFlash_Device_t *Adafruit_FlashTransport_ESP32::getFlashDevice(void) { - if (!_partition) { - return NULL; - } - - // Limit to partition size - _flash_device.total_size = _partition->size; - - esp_flash_t const *flash = _partition->flash_chip; - _flash_device.manufacturer_id = (flash->chip_id >> 16); - _flash_device.memory_type = (flash->chip_id >> 8) & 0xff; - _flash_device.capacity = flash->chip_id & 0xff; - - return &_flash_device; -} - -void Adafruit_FlashTransport_ESP32::setClockSpeed(uint32_t write_hz, - uint32_t read_hz) { - // do nothing, just use current configured clock - (void)write_hz; - (void)read_hz; -} - -bool Adafruit_FlashTransport_ESP32::runCommand(uint8_t command) { - switch (command) { - case SFLASH_CMD_ERASE_CHIP: - return ESP_OK == esp_partition_erase_range(_partition, 0, _partition->size); - - // do nothing, mostly write enable - default: - return true; - } -} - -bool Adafruit_FlashTransport_ESP32::readCommand(uint8_t command, - uint8_t *response, - uint32_t len) { - // mostly is Read STATUS, just fill with 0x0 - (void)command; - memset(response, 0, len); - - return true; -} - -bool Adafruit_FlashTransport_ESP32::writeCommand(uint8_t command, - uint8_t const *data, - uint32_t len) { - // mostly is Write Status, do nothing - (void)command; - (void)data; - (void)len; - - return true; -} - -bool Adafruit_FlashTransport_ESP32::eraseCommand(uint8_t command, - uint32_t addr) { - uint32_t erase_sz; - - if (command == SFLASH_CMD_ERASE_SECTOR) { - erase_sz = SFLASH_SECTOR_SIZE; - } else if (command == SFLASH_CMD_ERASE_BLOCK) { - erase_sz = SFLASH_BLOCK_SIZE; - } else { - return false; - } - - return ESP_OK == esp_partition_erase_range(_partition, addr, erase_sz); -} - -bool Adafruit_FlashTransport_ESP32::readMemory(uint32_t addr, uint8_t *data, - uint32_t len) { - return ESP_OK == esp_partition_read(_partition, addr, data, len); -} - -bool Adafruit_FlashTransport_ESP32::writeMemory(uint32_t addr, - uint8_t const *data, - uint32_t len) { - return ESP_OK == esp_partition_write(_partition, addr, data, len); -} - -#endif diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.h b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.h deleted file mode 100644 index 98b0fd01..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_ESP32.h +++ /dev/null @@ -1,60 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2020 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef ADAFRUIT_FLASHTRANSPORT_ESP32_H_ -#define ADAFRUIT_FLASHTRANSPORT_ESP32_H_ - -#include "Arduino.h" -#include "SPI.h" -#include "flash_devices.h" - -class Adafruit_FlashTransport_ESP32 : public Adafruit_FlashTransport { -private: - esp_partition_t const *_partition; - SPIFlash_Device_t _flash_device; - -public: - Adafruit_FlashTransport_ESP32(void); - - virtual void begin(void); - virtual void end(void); - - virtual bool supportQuadMode(void) { return false; } - - virtual void setClockSpeed(uint32_t write_hz, uint32_t read_hz); - - virtual bool runCommand(uint8_t command); - virtual bool readCommand(uint8_t command, uint8_t *response, uint32_t len); - virtual bool writeCommand(uint8_t command, uint8_t const *data, uint32_t len); - virtual bool eraseCommand(uint8_t command, uint32_t addr); - - virtual bool readMemory(uint32_t addr, uint8_t *data, uint32_t len); - virtual bool writeMemory(uint32_t addr, uint8_t const *data, uint32_t len); - - // Flash device is already detected and configured, get the pointer without - // go through initial sequence - SPIFlash_Device_t *getFlashDevice(void); -}; - -#endif /* ADAFRUIT_FLASHTRANSPORT_ESP32_H_ */ diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI.h b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI.h deleted file mode 100644 index 18a651fb..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI.h +++ /dev/null @@ -1,54 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2019 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef ADAFRUIT_FLASHTRANSPORT_QSPI_H_ -#define ADAFRUIT_FLASHTRANSPORT_QSPI_H_ - -class Adafruit_FlashTransport_QSPI : public Adafruit_FlashTransport { -private: - int8_t _sck, _cs; - int8_t _io0, _io1, _io2, _io3; - -public: - Adafruit_FlashTransport_QSPI(int8_t pinSCK, int8_t pinCS, int8_t pinIO0, - int8_t pinIO1, int8_t pinIO2, int8_t pinIO3); - Adafruit_FlashTransport_QSPI(void); - - virtual void begin(void); - virtual void end(void); - - virtual bool supportQuadMode(void) { return true; } - - virtual void setClockSpeed(uint32_t write_hz, uint32_t read_hz); - - virtual bool runCommand(uint8_t command); - virtual bool readCommand(uint8_t command, uint8_t *response, uint32_t len); - virtual bool writeCommand(uint8_t command, uint8_t const *data, uint32_t len); - - virtual bool eraseCommand(uint8_t command, uint32_t address); - virtual bool readMemory(uint32_t addr, uint8_t *data, uint32_t len); - virtual bool writeMemory(uint32_t addr, uint8_t const *data, uint32_t len); -}; - -#endif /* ADAFRUIT_FLASHTRANSPORT_QSPI_H_ */ diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_NRF.cpp b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_NRF.cpp deleted file mode 100644 index bb2e2a3e..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_NRF.cpp +++ /dev/null @@ -1,267 +0,0 @@ -/** - * The MIT License (MIT) - * - * Copyright (c) 2019 Ha Thach for Adafruit Industries LLC - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifdef NRF52840_XXAA - -#include "Adafruit_FlashTransport.h" -#include "nrfx_qspi.h" -#include - -// default PIN_QSPI_SCK, CS, IO0, IO1, IO2, IO3 to -1 to build with non-qpsi -// board -#ifndef PIN_QSPI_SCK -#define PIN_QSPI_SCK -1 -#endif - -#ifndef PIN_QSPI_CS -#define PIN_QSPI_CS -1 -#endif - -#ifndef PIN_QSPI_IO0 -#define PIN_QSPI_IO0 -1 -#endif - -#ifndef PIN_QSPI_IO1 -#define PIN_QSPI_IO1 -1 -#endif - -#ifndef PIN_QSPI_IO2 -#define PIN_QSPI_IO2 -1 -#endif - -#ifndef PIN_QSPI_IO3 -#define PIN_QSPI_IO3 -1 -#endif - -Adafruit_FlashTransport_QSPI::Adafruit_FlashTransport_QSPI(void) - : Adafruit_FlashTransport_QSPI(PIN_QSPI_SCK, PIN_QSPI_CS, PIN_QSPI_IO0, - PIN_QSPI_IO1, PIN_QSPI_IO2, PIN_QSPI_IO3) {} - -Adafruit_FlashTransport_QSPI::Adafruit_FlashTransport_QSPI( - int8_t sck, int8_t cs, int8_t io0, int8_t io1, int8_t io2, int8_t io3) { - _addr_len = 3; // work with most device if not set - _cmd_read = SFLASH_CMD_QUAD_READ; - _sck = sck; - _cs = cs; - _io0 = io0; - _io1 = io1; - _io2 = io2; - _io3 = io3; -} - -void Adafruit_FlashTransport_QSPI::begin(void) { - // Init QSPI flash - nrfx_qspi_config_t qspi_cfg = { - .xip_offset = 0, - .pins = - { - .sck_pin = (uint8_t)g_ADigitalPinMap[_sck], - .csn_pin = (uint8_t)g_ADigitalPinMap[_cs], - .io0_pin = (uint8_t)g_ADigitalPinMap[_io0], - .io1_pin = (uint8_t)g_ADigitalPinMap[_io1], - .io2_pin = (uint8_t)g_ADigitalPinMap[_io2], - .io3_pin = (uint8_t)g_ADigitalPinMap[_io3], - }, - .prot_if = {.readoc = NRF_QSPI_READOC_READ4O, // 0x6B read command - .writeoc = NRF_QSPI_WRITEOC_PP4O, // 0x32 write command - .addrmode = NRF_QSPI_ADDRMODE_24BIT, - .dpmconfig = false}, - .phy_if = - { - .sck_delay = 10, - .dpmen = false, - .spi_mode = NRF_QSPI_MODE_0, - .sck_freq = NRF_QSPI_FREQ_32MDIV16, // start with low 2 Mhz speed - }, - .irq_priority = 7}; - - // No callback for blocking API - nrfx_qspi_init(&qspi_cfg, NULL, NULL); -} - -void Adafruit_FlashTransport_QSPI::end(void) { nrfx_qspi_uninit(); } - -void Adafruit_FlashTransport_QSPI::setClockSpeed(uint32_t clock_hz, - uint32_t read_hz) { - (void)read_hz; - // Start at 16 MHz and go down. - // At 32 MHz nRF52840 doesn't work reliably !!! - // clkdiv = 0 is 32 Mhz - // clkdiv = 1 is 16 MHz, etc. - uint8_t clkdiv = 1; - while ((32000000UL / (clkdiv + 1) > clock_hz) && (clkdiv < 16)) { - clkdiv++; - } - - // delay is set to one freq period - uint8_t delay = 1; - - if (clkdiv) { - delay = (1UL << (clkdiv - 1)); - } - - NRF_QSPI->IFCONFIG1 &= - ~(QSPI_IFCONFIG1_SCKFREQ_Msk | QSPI_IFCONFIG1_SCKDELAY_Msk); - NRF_QSPI->IFCONFIG1 |= (clkdiv << QSPI_IFCONFIG1_SCKFREQ_Pos) | - (delay << QSPI_IFCONFIG1_SCKDELAY_Pos); -} - -bool Adafruit_FlashTransport_QSPI::runCommand(uint8_t command) { - nrf_qspi_cinstr_conf_t cinstr_cfg = {.opcode = command, - .length = NRF_QSPI_CINSTR_LEN_1B, - .io2_level = true, - .io3_level = true, - .wipwait = false, - .wren = false}; - - return nrfx_qspi_cinstr_xfer(&cinstr_cfg, NULL, NULL) == NRFX_SUCCESS; -} - -bool Adafruit_FlashTransport_QSPI::readCommand(uint8_t command, - uint8_t *response, - uint32_t len) { - nrf_qspi_cinstr_conf_t cinstr_cfg = {.opcode = command, - .length = - (nrf_qspi_cinstr_len_t)(len + 1), - .io2_level = true, - .io3_level = true, - .wipwait = false, - .wren = false}; - return nrfx_qspi_cinstr_xfer(&cinstr_cfg, NULL, response) == NRFX_SUCCESS; -} - -bool Adafruit_FlashTransport_QSPI::writeCommand(uint8_t command, - uint8_t const *data, - uint32_t len) { - nrf_qspi_cinstr_conf_t cinstr_cfg = { - .opcode = command, - .length = (nrf_qspi_cinstr_len_t)(len + 1), - .io2_level = true, - .io3_level = true, - .wipwait = false, - .wren = false // We do this manually. - }; - return nrfx_qspi_cinstr_xfer(&cinstr_cfg, data, NULL) == NRFX_SUCCESS; -} - -bool Adafruit_FlashTransport_QSPI::eraseCommand(uint8_t command, - uint32_t address) { - nrf_qspi_erase_len_t erase_len; - - if (command == SFLASH_CMD_ERASE_SECTOR) { - erase_len = NRF_QSPI_ERASE_LEN_4KB; - } else if (command == SFLASH_CMD_ERASE_BLOCK) { - erase_len = NRF_QSPI_ERASE_LEN_64KB; - } else { - return false; - } - - return NRFX_SUCCESS == nrfx_qspi_erase(erase_len, address); -} - -//--------------------------------------------------------------------+ -// Read & Write -//--------------------------------------------------------------------+ -static uint32_t read_write_odd(bool read_op, uint32_t addr, uint8_t *data, - uint32_t len) { - uint8_t buf4[4] __attribute__((aligned(4))); - uint32_t count = 4 - (((uint32_t)data) & 0x03); - count = min(count, len); - - if (read_op) { - if (NRFX_SUCCESS != nrfx_qspi_read(buf4, 4, addr)) { - return 0; - } - - memcpy(data, buf4, count); - } else { - memset(buf4, 0xff, 4); - memcpy(buf4, data, count); - - if (NRFX_SUCCESS != nrfx_qspi_write(buf4, 4, addr)) { - return 0; - } - } - - return count; -} - -static bool read_write_memory(bool read_op, uint32_t addr, uint8_t *data, - uint32_t len) { - // buffer is not 4-byte aligned - if (((uint32_t)data) & 3) { - uint32_t const count = read_write_odd(read_op, addr, data, len); - if (!count) { - return false; - } - - data += count; - addr += count; - len -= count; - } - - // nrfx_qspi_read works in 4 byte increments, though it doesn't - // signal an error if sz is not a multiple of 4. Read (directly into data) - // all but the last 1, 2, or 3 bytes depending on the (remaining) length. - if (len > 3) { - uint32_t const len4 = len & ~(uint32_t)3; - - if (read_op) { - if (NRFX_SUCCESS != nrfx_qspi_read(data, len4, addr)) { - return 0; - } - } else { - if (NRFX_SUCCESS != nrfx_qspi_write(data, len4, addr)) { - return 0; - } - } - - data += len4; - addr += len4; - len -= len4; - } - - // Now, if we have any bytes left over, we must do a final read of 4 - // bytes and copy 1, 2, or 3 bytes to data. - if (len) { - if (!read_write_odd(read_op, addr, data, len)) { - return false; - } - } - - return true; -} - -bool Adafruit_FlashTransport_QSPI::readMemory(uint32_t addr, uint8_t *data, - uint32_t len) { - return read_write_memory(true, addr, data, len); -} - -bool Adafruit_FlashTransport_QSPI::writeMemory(uint32_t addr, - uint8_t const *data, - uint32_t len) { - return read_write_memory(false, addr, (uint8_t *)data, len); -} - -#endif diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_SAMD.cpp b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_SAMD.cpp deleted file mode 100644 index 9e0546f2..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_QSPI_SAMD.cpp +++ /dev/null @@ -1,235 +0,0 @@ -/** - * The MIT License (MIT) - * - * Copyright (c) 2019 Ha Thach and Dean Miller for Adafruit Industries LLC - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#ifdef __SAMD51__ - -#include "Adafruit_FlashTransport.h" -#include "wiring_private.h" -#include - -static void _run_instruction(uint8_t command, uint32_t iframe, uint32_t addr, - uint8_t *buffer, uint32_t size); - -// Turn off cache and invalidate all data in it. -static void samd_peripherals_disable_and_clear_cache(void) { - CMCC->CTRL.bit.CEN = 0; - while (CMCC->SR.bit.CSTS) { - } - CMCC->MAINT0.bit.INVALL = 1; -} - -// Enable cache -static void samd_peripherals_enable_cache(void) { CMCC->CTRL.bit.CEN = 1; } - -Adafruit_FlashTransport_QSPI::Adafruit_FlashTransport_QSPI(void) - : Adafruit_FlashTransport_QSPI(PIN_QSPI_SCK, PIN_QSPI_CS, PIN_QSPI_IO0, - PIN_QSPI_IO1, PIN_QSPI_IO2, PIN_QSPI_IO3) {} - -Adafruit_FlashTransport_QSPI::Adafruit_FlashTransport_QSPI( - int8_t sck, int8_t cs, int8_t io0, int8_t io1, int8_t io2, int8_t io3) { - _addr_len = 3; // work with most device if not set - _cmd_read = SFLASH_CMD_QUAD_READ; - _sck = sck; - _cs = cs; - _io0 = io0; - _io1 = io1; - _io2 = io2; - _io3 = io3; -} - -void Adafruit_FlashTransport_QSPI::begin(void) { - MCLK->APBCMASK.bit.QSPI_ = true; - MCLK->AHBMASK.bit.QSPI_ = true; - MCLK->AHBMASK.bit.QSPI_2X_ = false; // Only true if we are doing DDR. - - QSPI->CTRLA.bit.SWRST = 1; - - // set all pins to QSPI periph - pinPeripheral(_sck, PIO_COM); - pinPeripheral(_cs, PIO_COM); - pinPeripheral(_io0, PIO_COM); - pinPeripheral(_io1, PIO_COM); - pinPeripheral(_io2, PIO_COM); - pinPeripheral(_io3, PIO_COM); - - QSPI->BAUD.reg = - QSPI_BAUD_BAUD(VARIANT_MCK / 4000000UL); // start with low 4Mhz, Mode 0 - QSPI->CTRLB.reg = QSPI_CTRLB_MODE_MEMORY | QSPI_CTRLB_DATALEN_8BITS | - QSPI_CTRLB_CSMODE_LASTXFER; - - QSPI->CTRLA.bit.ENABLE = 1; -} - -void Adafruit_FlashTransport_QSPI::end(void) { - QSPI->CTRLA.bit.ENABLE = 0; - - MCLK->APBCMASK.bit.QSPI_ = false; - MCLK->AHBMASK.bit.QSPI_ = false; - MCLK->AHBMASK.bit.QSPI_2X_ = false; -} - -bool Adafruit_FlashTransport_QSPI::runCommand(uint8_t command) { - uint32_t iframe = QSPI_INSTRFRAME_WIDTH_SINGLE_BIT_SPI | - QSPI_INSTRFRAME_ADDRLEN_24BITS | - QSPI_INSTRFRAME_TFRTYPE_READ | QSPI_INSTRFRAME_INSTREN; - - _run_instruction(command, iframe, 0, NULL, 0); - return true; -} - -bool Adafruit_FlashTransport_QSPI::readCommand(uint8_t command, - uint8_t *response, - uint32_t len) { - uint32_t iframe = QSPI_INSTRFRAME_WIDTH_SINGLE_BIT_SPI | - QSPI_INSTRFRAME_ADDRLEN_24BITS | - QSPI_INSTRFRAME_TFRTYPE_READ | QSPI_INSTRFRAME_INSTREN | - QSPI_INSTRFRAME_DATAEN; - - samd_peripherals_disable_and_clear_cache(); - _run_instruction(command, iframe, 0, response, len); - samd_peripherals_enable_cache(); - - return true; -} - -bool Adafruit_FlashTransport_QSPI::writeCommand(uint8_t command, - uint8_t const *data, - uint32_t len) { - uint32_t iframe = QSPI_INSTRFRAME_WIDTH_SINGLE_BIT_SPI | - QSPI_INSTRFRAME_ADDRLEN_24BITS | - QSPI_INSTRFRAME_TFRTYPE_WRITE | QSPI_INSTRFRAME_INSTREN | - (data != NULL ? QSPI_INSTRFRAME_DATAEN : 0); - - samd_peripherals_disable_and_clear_cache(); - _run_instruction(command, iframe, 0, (uint8_t *)data, len); - samd_peripherals_enable_cache(); - - return true; -} - -bool Adafruit_FlashTransport_QSPI::eraseCommand(uint8_t command, - uint32_t address) { - // Sector Erase - uint32_t iframe = QSPI_INSTRFRAME_WIDTH_SINGLE_BIT_SPI | - QSPI_INSTRFRAME_ADDRLEN_24BITS | - QSPI_INSTRFRAME_TFRTYPE_WRITE | QSPI_INSTRFRAME_INSTREN | - QSPI_INSTRFRAME_ADDREN; - - _run_instruction(command, iframe, address, NULL, 0); - return true; -} - -bool Adafruit_FlashTransport_QSPI::readMemory(uint32_t addr, uint8_t *data, - uint32_t len) { - // Command 0x6B 1 line address, 4 line Data - // Quad output mode, read memory type - uint32_t iframe = QSPI_INSTRFRAME_WIDTH_QUAD_OUTPUT | - QSPI_INSTRFRAME_ADDRLEN_24BITS | - QSPI_INSTRFRAME_TFRTYPE_READMEMORY | - QSPI_INSTRFRAME_INSTREN | QSPI_INSTRFRAME_ADDREN | - QSPI_INSTRFRAME_DATAEN | QSPI_INSTRFRAME_DUMMYLEN(8); - - samd_peripherals_disable_and_clear_cache(); - _run_instruction(SFLASH_CMD_QUAD_READ, iframe, addr, data, len); - samd_peripherals_enable_cache(); - - return true; -} - -bool Adafruit_FlashTransport_QSPI::writeMemory(uint32_t addr, - uint8_t const *data, - uint32_t len) { - uint32_t iframe = - QSPI_INSTRFRAME_WIDTH_QUAD_OUTPUT | QSPI_INSTRFRAME_ADDRLEN_24BITS | - QSPI_INSTRFRAME_TFRTYPE_WRITEMEMORY | QSPI_INSTRFRAME_INSTREN | - QSPI_INSTRFRAME_ADDREN | QSPI_INSTRFRAME_DATAEN; - - samd_peripherals_disable_and_clear_cache(); - _run_instruction(SFLASH_CMD_QUAD_PAGE_PROGRAM, iframe, addr, (uint8_t *)data, - len); - samd_peripherals_enable_cache(); - - return true; -} - -/**************************************************************************/ -/*! - @brief set the clock speed - @param clock_hz clock speed in hertz - */ -/**************************************************************************/ -void Adafruit_FlashTransport_QSPI::setClockSpeed(uint32_t clock_hz, - uint32_t read_hz) { - (void)read_hz; - QSPI->BAUD.bit.BAUD = VARIANT_MCK / clock_hz; -} - -/**************************************************************************/ -/*! - @brief Run a single QSPI instruction. - @param command instruction code - @param iframe iframe register value (configured by caller according to command - code) - @param addr the address to read or write from. If the instruction doesn't - require an address, this parameter is meaningless. - @param buffer pointer to the data to be written or stored depending on the type - is Read or Write - @param size the number of bytes to read or write. - */ -/**************************************************************************/ -static void _run_instruction(uint8_t command, uint32_t iframe, uint32_t addr, - uint8_t *buffer, uint32_t size) { - if (command == SFLASH_CMD_ERASE_SECTOR || command == SFLASH_CMD_ERASE_BLOCK) { - QSPI->INSTRADDR.reg = addr; - } - - QSPI->INSTRCTRL.bit.INSTR = command; - QSPI->INSTRFRAME.reg = iframe; - - // Dummy read of INSTRFRAME needed to synchronize. - // See Instruction Transmission Flow Diagram, figure 37.9, page 995 - // and Example 4, page 998, section 37.6.8.5. - volatile uint32_t dummy = QSPI->INSTRFRAME.reg; - (void)dummy; - - if (buffer && size) { - uint8_t *qspi_mem = (uint8_t *)(QSPI_AHB + addr); - uint32_t const tfr_type = iframe & QSPI_INSTRFRAME_TFRTYPE_Msk; - - if ((tfr_type == QSPI_INSTRFRAME_TFRTYPE_READ) || - (tfr_type == QSPI_INSTRFRAME_TFRTYPE_READMEMORY)) { - memcpy(buffer, qspi_mem, size); - } else { - memcpy(qspi_mem, buffer, size); - } - } - - QSPI->CTRLA.reg = QSPI_CTRLA_ENABLE | QSPI_CTRLA_LASTXFER; - - while (!QSPI->INTFLAG.bit.INSTREND) { - yield(); - } - QSPI->INTFLAG.bit.INSTREND = 1; -} - -#endif diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.cpp b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.cpp deleted file mode 100644 index bb53a299..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.cpp +++ /dev/null @@ -1,218 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2020 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "Adafruit_FlashTransport.h" - -#ifdef ARDUINO_ARCH_RP2040 - -#include - -// symbol from linker that matches 'Tools->Flash Size' menu selection for -// filesystem -extern uint8_t _FS_start; -extern uint8_t _FS_end; - -// FS Size determined by menu selection -#define MENU_FS_SIZE ((uint32_t)(&_FS_end - &_FS_start)) - -// CircuitPython partition scheme with -// - start address = 1 MB (Pico), 1.5 MB (Pico W) -// - size = total flash - 1 MB + 4KB (since CPY does not reserve EEPROM from -// arduino core) -#if defined(ARDUINO_RASPBERRY_PI_PICO_W) -const uint32_t Adafruit_FlashTransport_RP2040::CPY_START_ADDR = (1536 * 1024); -#else -const uint32_t Adafruit_FlashTransport_RP2040::CPY_START_ADDR = - (1 * 1024 * 1024); -#endif -const uint32_t Adafruit_FlashTransport_RP2040::CPY_SIZE = - (((uint32_t)&_FS_end) - - (XIP_BASE + Adafruit_FlashTransport_RP2040::CPY_START_ADDR) + 4096); - -static inline void fl_lock(bool idle) { - noInterrupts(); - if (idle) - rp2040.idleOtherCore(); -} - -static inline void fl_unlock(bool idle) { - if (idle) - rp2040.resumeOtherCore(); - interrupts(); -} - -Adafruit_FlashTransport_RP2040::Adafruit_FlashTransport_RP2040( - uint32_t start_addr, uint32_t size, bool idle) - : _idle_other_core_on_write(idle) { - _cmd_read = SFLASH_CMD_READ; - _addr_len = 3; // work with most device if not set - - _start_addr = start_addr; - _size = size; - - // detect start address and size that match menu option - if (!_start_addr) { - _start_addr = (uint32_t)&_FS_start - XIP_BASE; - } - - if (!_size) { - _size = MENU_FS_SIZE; - } - - memset(&_flash_dev, 0, sizeof(_flash_dev)); -} - -void Adafruit_FlashTransport_RP2040::begin(void) { - _flash_dev.total_size = _size; - -#if 0 - // Read the RDID register only for JEDEC ID - uint8_t const cmd[] = { - 0x9f, - 0, - 0, - 0, - }; - uint8_t data[4]; - fl_lock(_idle_other_core_on_write); - flash_do_cmd(cmd, data, 5); - fl_unlock(_idle_other_core_on_write); - - uint8_t *jedec_ids = data + 1; - - _flash_dev.manufacturer_id = jedec_ids[0]; - _flash_dev.memory_type = jedec_ids[1]; - _flash_dev.capacity = jedec_ids[2]; -#else - // skip JEDEC ID - _flash_dev.manufacturer_id = 0xad; - _flash_dev.memory_type = 0xaf; - _flash_dev.capacity = 0x00; -#endif -} - -void Adafruit_FlashTransport_RP2040::end(void) { - _cmd_read = SFLASH_CMD_READ; - _addr_len = 3; // work with most device if not set -} - -SPIFlash_Device_t *Adafruit_FlashTransport_RP2040::getFlashDevice(void) { - return &_flash_dev; -} - -void Adafruit_FlashTransport_RP2040::setClockSpeed(uint32_t write_hz, - uint32_t read_hz) { - // do nothing, just use current configured clock - (void)write_hz; - (void)read_hz; -} - -bool Adafruit_FlashTransport_RP2040::runCommand(uint8_t command) { - if (_size < 4096) { - return false; - } - - switch (command) { - case SFLASH_CMD_ERASE_CHIP: - fl_lock(_idle_other_core_on_write); - flash_range_erase(_start_addr, _size); - fl_unlock(_idle_other_core_on_write); - break; - - // do nothing, mostly write enable - default: - break; - } - - return true; -} - -bool Adafruit_FlashTransport_RP2040::readCommand(uint8_t command, - uint8_t *response, - uint32_t len) { - // mostly is Read STATUS, just fill with 0x0 - (void)command; - memset(response, 0, len); - - return true; -} - -bool Adafruit_FlashTransport_RP2040::writeCommand(uint8_t command, - uint8_t const *data, - uint32_t len) { - // mostly is Write Status, do nothing - (void)command; - (void)data; - (void)len; - - return true; -} - -bool Adafruit_FlashTransport_RP2040::eraseCommand(uint8_t command, - uint32_t addr) { - uint32_t erase_sz; - - if (command == SFLASH_CMD_ERASE_SECTOR) { - erase_sz = SFLASH_SECTOR_SIZE; - } else if (command == SFLASH_CMD_ERASE_BLOCK) { - erase_sz = SFLASH_BLOCK_SIZE; - } else { - return false; - } - - if (!check_addr(addr + erase_sz)) { - return false; - } - - fl_lock(_idle_other_core_on_write); - flash_range_erase(_start_addr + addr, erase_sz); - fl_unlock(_idle_other_core_on_write); - - return true; -} - -bool Adafruit_FlashTransport_RP2040::readMemory(uint32_t addr, uint8_t *data, - uint32_t len) { - if (!check_addr(addr + len)) { - return false; - } - - memcpy(data, (void *)(XIP_BASE + _start_addr + addr), len); - return true; -} - -bool Adafruit_FlashTransport_RP2040::writeMemory(uint32_t addr, - uint8_t const *data, - uint32_t len) { - if (!check_addr(addr + len)) { - return false; - } - - fl_lock(_idle_other_core_on_write); - flash_range_program(_start_addr + addr, data, len); - fl_unlock(_idle_other_core_on_write); - return true; -} - -#endif diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.h b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.h deleted file mode 100644 index d91b1e2e..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_RP2040.h +++ /dev/null @@ -1,90 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2020 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef ADAFRUIT_FLASHTRANSPORT_RP2040_H_ -#define ADAFRUIT_FLASHTRANSPORT_RP2040_H_ - -#include "Arduino.h" -#include "SPI.h" -#include "flash_devices.h" - -class Adafruit_FlashTransport_RP2040 : public Adafruit_FlashTransport { -protected: - uint32_t _start_addr; - uint32_t _size; - SPIFlash_Device_t _flash_dev; - - // check if relative addr is valid - bool check_addr(uint32_t addr) { return addr <= _size; } - - // Flag (set via constructor) indicates whether other core must pause when - // writing or erasing flash. Default state is true, pause other core. VERY - // rare that this needs changed, REQUIRES SPECIAL LINKER CONFIG to locate - // ALL functions of other core entirely in RAM, else hard crash and flash - // filesystem corruption. PicoDVI is a rare use case. - bool _idle_other_core_on_write; - -public: - static const uint32_t CPY_START_ADDR; - static const uint32_t CPY_SIZE; - - // Generic constructor with address and size. If start_address and size are 0, - // value that matches filesystem setting in 'Tools->Flash Size' menu selection - // will be used. - // - // To be compatible with CircuitPython partition scheme (start_address = 1 - // MB, size = total flash - 1 MB) use - // Adafruit_FlashTransport_RP2040(CPY_START_ADDR, CPY_SIZE) - Adafruit_FlashTransport_RP2040(uint32_t start_addr = 0, uint32_t size = 0, - bool idle = true); - - virtual void begin(void); - virtual void end(void); - - virtual bool supportQuadMode(void) { return false; } - - virtual void setClockSpeed(uint32_t write_hz, uint32_t read_hz); - - virtual bool runCommand(uint8_t command); - virtual bool readCommand(uint8_t command, uint8_t *response, uint32_t len); - virtual bool writeCommand(uint8_t command, uint8_t const *data, uint32_t len); - virtual bool eraseCommand(uint8_t command, uint32_t addr); - - virtual bool readMemory(uint32_t addr, uint8_t *data, uint32_t len); - virtual bool writeMemory(uint32_t addr, uint8_t const *data, uint32_t len); - - // Flash device is already detected and configured, get the pointer without - // go through initial sequence - SPIFlash_Device_t *getFlashDevice(void); -}; - -class Adafruit_FlashTransport_RP2040_CPY - : public Adafruit_FlashTransport_RP2040 { - -public: - Adafruit_FlashTransport_RP2040_CPY(bool idle = true) - : Adafruit_FlashTransport_RP2040(CPY_START_ADDR, CPY_SIZE, idle) {} -}; - -#endif diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.cpp b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.cpp deleted file mode 100644 index ecb248c9..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.cpp +++ /dev/null @@ -1,181 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2019 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "Adafruit_FlashTransport.h" - -Adafruit_FlashTransport_SPI::Adafruit_FlashTransport_SPI( - uint8_t ss, SPIClass *spiinterface) { - _cmd_read = SFLASH_CMD_READ; - _addr_len = 3; // work with most device if not set - _ss = ss; - _spi = spiinterface; - _clock_wr = _clock_rd = 4000000; -} - -Adafruit_FlashTransport_SPI::Adafruit_FlashTransport_SPI(uint8_t ss, - SPIClass &spiinterface) - : Adafruit_FlashTransport_SPI(ss, &spiinterface) {} - -void Adafruit_FlashTransport_SPI::begin(void) { - pinMode(_ss, OUTPUT); - digitalWrite(_ss, HIGH); - - _spi->begin(); -} - -void Adafruit_FlashTransport_SPI::end(void) { - _spi->end(); - - pinMode(_ss, INPUT); -} - -void Adafruit_FlashTransport_SPI::setClockSpeed(uint32_t write_hz, - uint32_t read_hz) { - _clock_wr = write_hz; - _clock_rd = read_hz; -} - -bool Adafruit_FlashTransport_SPI::runCommand(uint8_t command) { - beginTransaction(_clock_wr); - - _spi->transfer(command); - - endTransaction(); - - return true; -} - -bool Adafruit_FlashTransport_SPI::readCommand(uint8_t command, - uint8_t *response, uint32_t len) { - beginTransaction(_clock_rd); - - _spi->transfer(command); - while (len--) { - *response++ = _spi->transfer(0xFF); - } - - endTransaction(); - - return true; -} - -bool Adafruit_FlashTransport_SPI::writeCommand(uint8_t command, - uint8_t const *data, - uint32_t len) { - beginTransaction(_clock_wr); - - _spi->transfer(command); - while (len--) { - (void)_spi->transfer(*data++); - } - - endTransaction(); - - return true; -} - -bool Adafruit_FlashTransport_SPI::eraseCommand(uint8_t command, uint32_t addr) { - beginTransaction(_clock_wr); - - uint8_t cmd_with_addr[5] = {command}; - fillAddress(cmd_with_addr + 1, addr); - - _spi->transfer(cmd_with_addr, 1 + _addr_len); - - endTransaction(); - - return true; -} - -void Adafruit_FlashTransport_SPI::fillAddress(uint8_t *buf, uint32_t addr) { - switch (_addr_len) { - case 4: - *buf++ = (addr >> 24) & 0xFF; - //__attribute__((fallthrough)); ESP32 doesn't support this attribute yet - // fall through - - case 3: - *buf++ = (addr >> 16) & 0xFF; - //__attribute__((fallthrough)); ESP32 doesn't support this attribute yet - // fall through - - case 2: - default: - *buf++ = (addr >> 8) & 0xFF; - *buf++ = addr & 0xFF; - } -} - -bool Adafruit_FlashTransport_SPI::readMemory(uint32_t addr, uint8_t *data, - uint32_t len) { - beginTransaction(_clock_rd); - - uint8_t cmd_with_addr[6] = {_cmd_read}; - fillAddress(cmd_with_addr + 1, addr); - - // Fast Read has 1 extra dummy byte - uint8_t const cmd_len = - 1 + _addr_len + (SFLASH_CMD_FAST_READ == _cmd_read ? 1 : 0); - - _spi->transfer(cmd_with_addr, cmd_len); - - // Use SPI DMA if available for best performance -#if defined(ARDUINO_NRF52_ADAFRUIT) && defined(NRF52840_XXAA) - _spi->transfer(NULL, data, len); -#elif defined(ARDUINO_ARCH_SAMD) && defined(_ADAFRUIT_ZERODMA_H_) - _spi->transfer(NULL, data, len, true); -#else - _spi->transfer(data, len); -#endif - - endTransaction(); - - return true; -} - -bool Adafruit_FlashTransport_SPI::writeMemory(uint32_t addr, - uint8_t const *data, - uint32_t len) { - beginTransaction(_clock_wr); - - uint8_t cmd_with_addr[5] = {SFLASH_CMD_PAGE_PROGRAM}; - fillAddress(cmd_with_addr + 1, addr); - - _spi->transfer(cmd_with_addr, 1 + _addr_len); - - // Use SPI DMA if available for best performance -#if defined(ARDUINO_NRF52_ADAFRUIT) && defined(NRF52840_XXAA) - _spi->transfer(data, NULL, len); -#elif defined(ARDUINO_ARCH_SAMD) && defined(_ADAFRUIT_ZERODMA_H_) - _spi->transfer(data, NULL, len, true); -#else - while (len--) { - _spi->transfer(*data++); - } -#endif - - endTransaction(); - - return true; -} diff --git a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.h b/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.h deleted file mode 100644 index 3437d437..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_FlashTransport_SPI.h +++ /dev/null @@ -1,73 +0,0 @@ -/* - * The MIT License (MIT) - * - * Copyright (c) 2019 hathach for Adafruit Industries - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef ADAFRUIT_FLASHTRANSPORT_SPI_H_ -#define ADAFRUIT_FLASHTRANSPORT_SPI_H_ - -#include "Arduino.h" -#include "SPI.h" - -class Adafruit_FlashTransport_SPI : public Adafruit_FlashTransport { -private: - SPIClass *_spi; - uint8_t _ss; - - // SAMD21 M0 can write up to 24 Mhz, but can only read reliably with 12 MHz - uint32_t _clock_wr; - uint32_t _clock_rd; - -public: - Adafruit_FlashTransport_SPI(uint8_t ss, SPIClass *spiinterface); - Adafruit_FlashTransport_SPI(uint8_t ss, SPIClass &spiinterface); - - virtual void begin(void); - virtual void end(void); - - virtual bool supportQuadMode(void) { return false; } - - virtual void setClockSpeed(uint32_t write_hz, uint32_t read_hz); - - virtual bool runCommand(uint8_t command); - virtual bool readCommand(uint8_t command, uint8_t *response, uint32_t len); - virtual bool writeCommand(uint8_t command, uint8_t const *data, uint32_t len); - virtual bool eraseCommand(uint8_t command, uint32_t addr); - - virtual bool readMemory(uint32_t addr, uint8_t *data, uint32_t len); - virtual bool writeMemory(uint32_t addr, uint8_t const *data, uint32_t len); - -private: - void fillAddress(uint8_t *buf, uint32_t addr); - - void beginTransaction(uint32_t clock_hz) { - _spi->beginTransaction(SPISettings(clock_hz, MSBFIRST, SPI_MODE0)); - digitalWrite(_ss, LOW); - } - - void endTransaction(void) { - digitalWrite(_ss, HIGH); - _spi->endTransaction(); - } -}; - -#endif /* ADAFRUIT_FLASHTRANSPORT_SPI_H_ */ diff --git a/src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.cpp b/src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.cpp deleted file mode 100644 index fa6833b5..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.cpp +++ /dev/null @@ -1,517 +0,0 @@ -/** - * The MIT License (MIT) - * - * Copyright (c) 2019 Ha Thach and Dean Miller for Adafruit Industries LLC - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#include "Adafruit_SPIFlashBase.h" -#include "pins_arduino.h" -#include - -#include "flash_devices.h" - -#if SPIFLASH_DEBUG -#define SPIFLASH_LOG(_address, _count) \ - do { \ - Serial.print(__FUNCTION__); \ - Serial.print(": address = "); \ - Serial.print(_address, HEX); \ - if (_count) { \ - Serial.print(" count = "); \ - Serial.print(_count); \ - } \ - Serial.println(); \ - } while (0) - -#else -#define SPIFLASH_LOG(_sector, _count) - -#endif - -Adafruit_SPIFlashBase::Adafruit_SPIFlashBase() { - _trans = NULL; - _flash_dev = NULL; - _ind_pin = -1; - _ind_active = true; -} - -Adafruit_SPIFlashBase::Adafruit_SPIFlashBase( - Adafruit_FlashTransport *transport) { - _trans = transport; - _flash_dev = NULL; - _ind_pin = -1; - _ind_active = true; -} - -#if defined(ARDUINO_ARCH_ESP32) || defined(ARDUINO_ARCH_RP2040) - -// For ESP32 and RP2040 the SPI flash is already detected and configured -// We could skip the initial sequence -bool Adafruit_SPIFlashBase::begin(SPIFlash_Device_t const *flash_devs, - size_t count) { - (void)flash_devs; - (void)count; - - if (_trans == NULL) { - return false; - } - - _trans->begin(); - -#if defined(ARDUINO_ARCH_ESP32) - _flash_dev = ((Adafruit_FlashTransport_ESP32 *)_trans)->getFlashDevice(); -#elif defined(ARDUINO_ARCH_RP2040) - _flash_dev = ((Adafruit_FlashTransport_RP2040 *)_trans)->getFlashDevice(); -#endif - - return true; -} - -#else - -/// List of all possible flash devices used by Adafruit boards -static const SPIFlash_Device_t possible_devices[] = { - // Main devices used in current Adafruit products - GD25Q16C, GD25Q32C, GD25Q64C, S25FL116K, S25FL216K, - - // Only a handful of production run - W25Q16FW, - - // Flash breakout - W25Q16JV_IQ, W25Q32JV_IQ, W25Q64JV_IQ, W25Q128JV_SQ, - - // Fujitsu FRAM - MB85RS64V, MB85RS1MT, MB85RS2MTA, MB85RS4MT, - - // Other common flash devices - AT25SF041, AT25DF081A}; - -/// Flash device list count -enum { - EXTERNAL_FLASH_DEVICE_COUNT = - sizeof(possible_devices) / sizeof(possible_devices[0]) -}; - -static SPIFlash_Device_t const *findDevice(SPIFlash_Device_t const *device_list, - int count, - uint8_t const (&jedec_ids)[4]) { - for (uint8_t i = 0; i < count; i++) { - const SPIFlash_Device_t *dev = &device_list[i]; - if (jedec_ids[0] == dev->manufacturer_id && - jedec_ids[1] == dev->memory_type && // comment to appease format check - jedec_ids[2] == dev->capacity) { - return dev; - } - } - return NULL; -} - -bool Adafruit_SPIFlashBase::begin(SPIFlash_Device_t const *flash_devs, - size_t count) { - if (_trans == NULL) { - return false; - } - - _trans->begin(); - - //------------- flash detection -------------// - // Note: Manufacturer can be assigned with numerous of continuation code - // (0x7F) - uint8_t jedec_ids[4]; - _trans->readCommand(SFLASH_CMD_READ_JEDEC_ID, jedec_ids, 4); - - // For simplicity with commonly used device, we only check for continuation - // code at 2nd byte (e.g Fujitsu FRAM devices) - if (jedec_ids[1] == 0x7F) { - // Shift and skip continuation code in 2nd byte - jedec_ids[1] = jedec_ids[2]; - jedec_ids[2] = jedec_ids[3]; - } - - // Check for device in supplied list, if any. - if (flash_devs != NULL) { - _flash_dev = findDevice(flash_devs, count, jedec_ids); - } - - // If not found, check for device in standard list. - if (_flash_dev == NULL) { - _flash_dev = - findDevice(possible_devices, EXTERNAL_FLASH_DEVICE_COUNT, jedec_ids); - } - - if (_flash_dev == NULL) { -#if SPIFLASH_DEBUG - Serial.print("Unknown flash device 0x"); - Serial.println( - ((uint32_t)jedec_ids[0]) << 16 | jedec_ids[1] << 8 | jedec_ids[2], HEX); -#endif - return false; - } - - // We don't know what state the flash is in so wait for any remaining writes - // and then reset (Skip this procedure for FRAM) - if (!_flash_dev->is_fram) { - - // The write in progress bit should be low. - while (readStatus() & 0x01) { - } - - // The suspended write/erase bit should be low. - if (!_flash_dev->single_status_byte) { - while (readStatus2() & 0x80) { - } - } - - _trans->runCommand(SFLASH_CMD_ENABLE_RESET); - _trans->runCommand(SFLASH_CMD_RESET); - - // Wait 30us for the reset - delayMicroseconds(30); - } - - // Speed up to max device frequency, or as high as possible - uint32_t wr_speed = _flash_dev->max_clock_speed_mhz * 1000000U; - -#ifdef F_CPU - // Limit to CPU speed if defined - wr_speed = min(wr_speed, (uint32_t)F_CPU); -#endif - - uint32_t rd_speed = wr_speed; - -#if defined(ARDUINO_ARCH_SAMD) && !defined(__SAMD51__) - // Hand-on testing show that SAMD21 M0 can write up to 24 Mhz, - // but only read reliably at 12 Mhz - rd_speed = min(12000000U, rd_speed); -#endif - - _trans->setClockSpeed(wr_speed, rd_speed); - - // Enable Quad Mode if available - if (_trans->supportQuadMode() && _flash_dev->supports_qspi) { - // Verify that QSPI mode is enabled. - uint8_t status = - _flash_dev->single_status_byte ? readStatus() : readStatus2(); - - // Check the quad enable bit. - if ((status & _flash_dev->quad_enable_bit_mask) == 0) { - writeEnable(); - - uint8_t full_status[2] = {0x00, _flash_dev->quad_enable_bit_mask}; - - if (_flash_dev->write_status_register_split) { - _trans->writeCommand(SFLASH_CMD_WRITE_STATUS2, full_status + 1, 1); - } else if (_flash_dev->single_status_byte) { - _trans->writeCommand(SFLASH_CMD_WRITE_STATUS, full_status + 1, 1); - } else { - _trans->writeCommand(SFLASH_CMD_WRITE_STATUS, full_status, 2); - } - } - } else { - // Single mode, use fast read if supported - if (_flash_dev->supports_fast_read) { - _trans->setReadCommand(SFLASH_CMD_FAST_READ); - } - } - - // Addressing byte depends on total size - uint8_t addr_byte; - if (_flash_dev->total_size > 16UL * 1024 * 1024) { - addr_byte = 4; - // Enable 4-Byte address mode (This has to be done after the reset above) - _trans->runCommand(SFLASH_CMD_4_BYTE_ADDR); - } else if (_flash_dev->total_size > 64UL * 1024) { - addr_byte = 3; - } else { - addr_byte = 2; - } - - _trans->setAddressLength(addr_byte); - - // Turn off sector protection if needed - // if (_flash_dev->has_sector_protection) - // { - // writeEnable(); - // - // uint8_t data[1] = {0x00}; - // QSPI0.writeCommand(QSPI_CMD_WRITE_STATUS, data, 1); - // } - - writeDisable(); - waitUntilReady(); - - return true; -} - -#endif // ARDUINO_ARCH_ESP32 - -void Adafruit_SPIFlashBase::end(void) { - if (_trans == NULL) { - return; - } - - _trans->end(); - _flash_dev = NULL; -} - -void Adafruit_SPIFlashBase::setIndicator(int pin, bool state_on) { - _ind_pin = pin; - _ind_active = state_on; -} - -uint32_t Adafruit_SPIFlashBase::size(void) { - return _flash_dev ? _flash_dev->total_size : 0; -} - -uint32_t Adafruit_SPIFlashBase::numPages(void) { - return _flash_dev ? _flash_dev->total_size / pageSize() : 0; -} - -uint16_t Adafruit_SPIFlashBase::pageSize(void) { return SFLASH_PAGE_SIZE; } - -uint32_t Adafruit_SPIFlashBase::getJEDECID(void) { - if (!_flash_dev) { - return 0xFFFFFF; - } else { - return (((uint32_t)_flash_dev->manufacturer_id) << 16) | - (_flash_dev->memory_type << 8) | _flash_dev->capacity; - } -} - -uint8_t Adafruit_SPIFlashBase::readStatus() { - uint8_t status; - _trans->readCommand(SFLASH_CMD_READ_STATUS, &status, 1); - return status; -} - -uint8_t Adafruit_SPIFlashBase::readStatus2(void) { - uint8_t status; - _trans->readCommand(SFLASH_CMD_READ_STATUS2, &status, 1); - return status; -} - -bool Adafruit_SPIFlashBase::isReady(void) { - if (_flash_dev->is_fram) { - return true; - } else { - return (readStatus() & 0x03) == 0; - } -} - -void Adafruit_SPIFlashBase::waitUntilReady(void) { - // FRAM has no need to wait for either read or write operation - if (_flash_dev->is_fram) { - return; - } - - // both WIP and WREN bit should be clear - while (readStatus() & 0x03) { - yield(); - } -} - -bool Adafruit_SPIFlashBase::writeEnable(void) { - return _trans->runCommand(SFLASH_CMD_WRITE_ENABLE); -} - -bool Adafruit_SPIFlashBase::writeDisable(void) { - return _trans->runCommand(SFLASH_CMD_WRITE_DISABLE); -} - -bool Adafruit_SPIFlashBase::erasePage(uint32_t pageNumber) { - if (!_flash_dev) { - return false; - } - - // skip erase for FRAM - if (_flash_dev->is_fram) { - return true; - } - - _indicator_on(); - - // Before we erase the page we need to wait for any writes to finish - waitUntilReady(); - writeEnable(); - - SPIFLASH_LOG(pageNumber * SFLASH_PAGE_SIZE, 0); - - bool const ret = _trans->eraseCommand(SFLASH_CMD_ERASE_PAGE, - pageNumber * SFLASH_PAGE_SIZE); - - _indicator_off(); - - return ret; -} - -bool Adafruit_SPIFlashBase::eraseSector(uint32_t sectorNumber) { - if (!_flash_dev) { - return false; - } - - // skip erase for FRAM - if (_flash_dev->is_fram) { - return true; - } - - _indicator_on(); - - // Before we erase the sector we need to wait for any writes to finish - waitUntilReady(); - writeEnable(); - - SPIFLASH_LOG(sectorNumber * SFLASH_SECTOR_SIZE, 0); - - bool const ret = _trans->eraseCommand(SFLASH_CMD_ERASE_SECTOR, - sectorNumber * SFLASH_SECTOR_SIZE); - - _indicator_off(); - - return ret; -} - -bool Adafruit_SPIFlashBase::eraseBlock(uint32_t blockNumber) { - if (!_flash_dev) { - return false; - } - - // skip erase for fram - if (_flash_dev->is_fram) { - return true; - } - - _indicator_on(); - - // Before we erase the sector we need to wait for any writes to finish - waitUntilReady(); - writeEnable(); - - bool const ret = _trans->eraseCommand(SFLASH_CMD_ERASE_BLOCK, - blockNumber * SFLASH_BLOCK_SIZE); - - _indicator_off(); - - return ret; -} - -bool Adafruit_SPIFlashBase::eraseChip(void) { - if (!_flash_dev) { - return false; - } - - // skip erase for fram - if (_flash_dev->is_fram) { - return true; - } - - _indicator_on(); - - // We need to wait for any writes to finish - waitUntilReady(); - writeEnable(); - - bool const ret = _trans->runCommand(SFLASH_CMD_ERASE_CHIP); - - _indicator_off(); - - return ret; -} - -uint32_t Adafruit_SPIFlashBase::readBuffer(uint32_t address, uint8_t *buffer, - uint32_t len) { - if (!_flash_dev) { - return 0; - } - - _indicator_on(); - - waitUntilReady(); - SPIFLASH_LOG(address, len); - bool const rc = _trans->readMemory(address, buffer, len); - - _indicator_off(); - - return rc ? len : 0; -} - -uint8_t Adafruit_SPIFlashBase::read8(uint32_t addr) { - uint8_t ret; - return readBuffer(addr, &ret, sizeof(ret)) ? ret : 0xff; -} - -uint16_t Adafruit_SPIFlashBase::read16(uint32_t addr) { - uint16_t ret; - return readBuffer(addr, (uint8_t *)&ret, sizeof(ret)) ? ret : 0xffff; -} - -uint32_t Adafruit_SPIFlashBase::read32(uint32_t addr) { - uint32_t ret; - return readBuffer(addr, (uint8_t *)&ret, sizeof(ret)) ? ret : 0xffffffff; -} - -uint32_t Adafruit_SPIFlashBase::writeBuffer(uint32_t address, - uint8_t const *buffer, - uint32_t len) { - if (!_flash_dev) { - return 0; - } - - SPIFLASH_LOG(address, len); - - _indicator_on(); - - // FRAM: the whole chip can be written in one pass without waiting. - // Also we need to explicitly disable WREN - if (_flash_dev->is_fram) { - writeEnable(); - - _trans->writeMemory(address, buffer, len); - - writeDisable(); - } else { - uint32_t remain = len; - - // write one page (256 bytes) at a time and - // must not go over page boundary - while (remain) { - waitUntilReady(); - writeEnable(); - - uint32_t const leftOnPage = - SFLASH_PAGE_SIZE - (address & (SFLASH_PAGE_SIZE - 1)); - uint32_t const toWrite = min(remain, leftOnPage); - - if (!_trans->writeMemory(address, buffer, toWrite)) { - break; - } - - remain -= toWrite; - buffer += toWrite; - address += toWrite; - } - - len -= remain; - } - - _indicator_off(); - - return len; -} diff --git a/src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.h b/src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.h deleted file mode 100644 index 2fab5466..00000000 --- a/src/madflight/bb/SPIFlash/Adafruit_SPIFlashBase.h +++ /dev/null @@ -1,96 +0,0 @@ -/** - * The MIT License (MIT) - * - * Copyright (c) 2019 Ha Thach and Dean Miller for Adafruit Industries LLC - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ - -#ifndef ADAFRUIT_SPIFLASHBASE_H_ -#define ADAFRUIT_SPIFLASHBASE_H_ - -#include "Adafruit_FlashTransport.h" -#include "flash_devices.h" - -// for debugging -#define SPIFLASH_DEBUG 0 - -// An easy to use interface for working with Flash memory. -// -// If you are managing allocation of the Flash space yourself, this is the -// class to use as it take very little RAM. -class Adafruit_SPIFlashBase { -public: - Adafruit_SPIFlashBase(); - Adafruit_SPIFlashBase(Adafruit_FlashTransport *transport); - ~Adafruit_SPIFlashBase() {} - - bool begin(SPIFlash_Device_t const *flash_devs = NULL, size_t count = 1); - void end(void); - - void setIndicator(int pin, bool state_on = true); - - uint32_t numPages(void); - uint16_t pageSize(void); - - uint32_t size(void); - - uint8_t readStatus(void); - uint8_t readStatus2(void); - void waitUntilReady(void); - bool writeEnable(void); - bool writeDisable(void); - bool isReady(void); // both WIP and WREN are clear - - uint32_t getJEDECID(void); - - uint32_t readBuffer(uint32_t address, uint8_t *buffer, uint32_t len); - uint32_t writeBuffer(uint32_t address, uint8_t const *buffer, uint32_t len); - - bool erasePage(uint32_t pageNumber); - bool eraseSector(uint32_t sectorNumber); - bool eraseBlock(uint32_t blockNumber); - bool eraseChip(void); - - // Helper - uint8_t read8(uint32_t addr); - uint16_t read16(uint32_t addr); - uint32_t read32(uint32_t addr); - -protected: - Adafruit_FlashTransport *_trans; - SPIFlash_Device_t const *_flash_dev; - - int _ind_pin; - bool _ind_active; - - void _indicator_on(void) { - if (_ind_pin >= 0) { - digitalWrite(_ind_pin, _ind_active ? HIGH : LOW); - } - } - - void _indicator_off(void) { - if (_ind_pin >= 0) { - digitalWrite(_ind_pin, _ind_active ? LOW : HIGH); - } - } -}; - -#endif /* ADAFRUIT_SPIFLASHBASE_H_ */ diff --git a/src/madflight/bb/SPIFlash/flash_devices.h b/src/madflight/bb/SPIFlash/flash_devices.h deleted file mode 100644 index 32767093..00000000 --- a/src/madflight/bb/SPIFlash/flash_devices.h +++ /dev/null @@ -1,542 +0,0 @@ -/* - * This file is part of the MicroPython project, http://micropython.org/ - * - * The MIT License (MIT) - * - * Copyright (c) 2018 Scott Shawcroft for Adafruit Industries LLC - * - * Permission is hereby granted, free of charge, to any person obtaining a copy - * of this software and associated documentation files (the "Software"), to deal - * in the Software without restriction, including without limitation the rights - * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell - * copies of the Software, and to permit persons to whom the Software is - * furnished to do so, subject to the following conditions: - * - * The above copyright notice and this permission notice shall be included in - * all copies or substantial portions of the Software. - * - * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR - * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, - * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE - * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER - * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, - * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN - * THE SOFTWARE. - */ -#ifndef MICROPY_INCLUDED_ATMEL_SAMD_EXTERNAL_FLASH_DEVICES_H -#define MICROPY_INCLUDED_ATMEL_SAMD_EXTERNAL_FLASH_DEVICES_H - -#include -#include - -typedef struct { - uint32_t total_size; - uint16_t start_up_time_us; - - // Three response bytes to 0x9f JEDEC ID command. - uint8_t manufacturer_id; - uint8_t memory_type; - uint8_t capacity; - - // Max clock speed for all operations and the fastest read mode. - uint8_t max_clock_speed_mhz; - - // Bitmask for Quad Enable bit if present. 0x00 otherwise. This is for the - // highest byte in the status register. - uint8_t quad_enable_bit_mask; - - bool has_sector_protection : 1; - - // Supports the 0x0b fast read command with 8 dummy cycles. - bool supports_fast_read : 1; - - // Supports the fast read, quad output command 0x6b with 8 dummy cycles. - bool supports_qspi : 1; - - // Supports the quad input page program command 0x32. This is known as 1-1-4 - // because it only uses all four lines for data. - bool supports_qspi_writes : 1; - - // Requires a separate command 0x31 to write to the second byte of the status - // register. Otherwise two byte are written via 0x01. - bool write_status_register_split : 1; - - // True when the status register is a single byte. This implies the Quad - // Enable bit is in the first byte and the Read Status Register 2 command - // (0x35) is unsupported. - bool single_status_byte : 1; - - // Fram does not need/support erase and has much simpler WRITE operation - bool is_fram : 1; - -} SPIFlash_Device_t; - -// Settings for the Adesto Tech AT25DF081A 1MiB SPI flash. Its on the SAMD21 -// Xplained board. -// Datasheet: https://www.adestotech.com/wp-content/uploads/doc8715.pdf -#define AT25DF081A \ - { \ - .total_size = (1UL << 20), /* 1 MiB */ \ - .start_up_time_us = 10000, .manufacturer_id = 0x1f, \ - .memory_type = 0x84, .capacity = 0x01, .max_clock_speed_mhz = 85, \ - .quad_enable_bit_mask = 0x00, .has_sector_protection = true, \ - .supports_fast_read = true, .supports_qspi = false, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Adesto Tech AT25SF041 4MiB SPI flash used in AS7262 sensor -#define AT25SF041 \ - { \ - .total_size = (4UL << 20), /* 4 MiB */ \ - .start_up_time_us = 10000, .manufacturer_id = 0x1f, \ - .memory_type = 0x45, .capacity = 0x01, .max_clock_speed_mhz = 85, \ - .quad_enable_bit_mask = 0x00, .has_sector_protection = true, \ - .supports_fast_read = true, .supports_qspi = false, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Gigadevice GD25Q16C 2MiB SPI flash. -// Datasheet: http://www.gigadevice.com/datasheet/gd25q16c/ -#define GD25Q16C \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc8, \ - .memory_type = 0x40, .capacity = 0x15, \ - .max_clock_speed_mhz = \ - 104, /* if we need 120 then we can turn on high performance mode */ \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Gigadevice GD25Q32C 4MiB SPI flash. -// Datasheet: http://www.gigadevice.com/datasheet/gd25q32c/ -#define GD25Q32C \ - { \ - .total_size = (1UL << 22), /* 4 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc8, \ - .memory_type = 0x40, .capacity = 0x16, \ - .max_clock_speed_mhz = \ - 104, /* if we need 120 then we can turn on high performance mode */ \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = true, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Gigadevice GD25Q64C 8MiB SPI flash. -// Datasheet: -// http://www.elm-tech.com/en/products/spi-flash-memory/gd25q64/gd25q64.pdf -#define GD25Q64C \ - { \ - .total_size = (1UL << 23), /* 8 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc8, \ - .memory_type = 0x40, .capacity = 0x17, \ - .max_clock_speed_mhz = \ - 104, /* if we need 120 then we can turn on high performance mode */ \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = true, \ - .single_status_byte = false, .is_fram = false, \ - } - -// https://www.fujitsu.com/uk/Images/MB85RS64V.pdf -// RDID has continuation code: 04-7F-03-02 -#define MB85RS64V \ - { \ - .total_size = 8UL * 1024, .start_up_time_us = 5000, \ - .manufacturer_id = 0x04, .memory_type = 0x03, .capacity = 0x02, \ - .max_clock_speed_mhz = 20, .quad_enable_bit_mask = 0x00, \ - .has_sector_protection = false, .supports_fast_read = false, \ - .supports_qspi = false, .supports_qspi_writes = false, \ - .write_status_register_split = false, .single_status_byte = true, \ - .is_fram = true, \ - } - -// https://www.fujitsu.com/uk/Images/MB85RS1MT.pdf -// RDID has continuation code: 04-7F-27-03 -#define MB85RS1MT \ - { \ - .total_size = 128UL * 1024, .start_up_time_us = 5000, \ - .manufacturer_id = 0x04, .memory_type = 0x27, .capacity = 0x03, \ - .max_clock_speed_mhz = 40, .quad_enable_bit_mask = 0x00, \ - .has_sector_protection = false, .supports_fast_read = true, \ - .supports_qspi = false, .supports_qspi_writes = false, \ - .write_status_register_split = false, .single_status_byte = true, \ - .is_fram = true, \ - } - -// https://www.fujitsu.com/uk/Images/MB85RS2MTA.pdf -// RDID has continuation code: 04-7F-48-03 -#define MB85RS2MTA \ - { \ - .total_size = 256UL * 1024, .start_up_time_us = 5000, \ - .manufacturer_id = 0x04, .memory_type = 0x48, .capacity = 0x03, \ - .max_clock_speed_mhz = 40, .quad_enable_bit_mask = 0x00, \ - .has_sector_protection = false, .supports_fast_read = true, \ - .supports_qspi = false, .supports_qspi_writes = false, \ - .write_status_register_split = false, .single_status_byte = true, \ - .is_fram = true, \ - } - -// https://www.fujitsu.com/uk/Images/MB85RS4MT.pdf -// RDID has continuation code: 04-7F-49-03 -#define MB85RS4MT \ - { \ - .total_size = 512UL * 1024, .start_up_time_us = 5000, \ - .manufacturer_id = 0x04, .memory_type = 0x49, .capacity = 0x03, \ - .max_clock_speed_mhz = 40, .quad_enable_bit_mask = 0x00, \ - .has_sector_protection = false, .supports_fast_read = true, \ - .supports_qspi = false, .supports_qspi_writes = true, \ - .write_status_register_split = false, .single_status_byte = true, \ - .is_fram = true, \ - } - -// Settings for the Macronix MX25L1606 2MiB SPI flash. -// Datasheet: -#define MX25L1606 \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc2, \ - .memory_type = 0x20, .capacity = 0x15, .max_clock_speed_mhz = 8, \ - .quad_enable_bit_mask = 0x40, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = true, .is_fram = false, \ - } - -// Settings for the Macronix MX25R1635F 2MiB SPI flash. -// Datasheet: -// https://www.macronix.com/Lists/Datasheet/Attachments/7595/MX25R1635F,%20Wide%20Range,%2016Mb,%20v1.6.pdf -// By default its in lower power mode which can only do 8mhz. In high power mode -// it can do 80mhz. -#define MX25R1635F \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc2, \ - .memory_type = 0x28, .capacity = 0x15, .max_clock_speed_mhz = 8, \ - .quad_enable_bit_mask = 0x40, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = true, .is_fram = false, \ - } - -// Settings for the Macronix MX25L3233F 4MiB SPI flash. -// Datasheet: -// http://www.macronix.com/Lists/Datasheet/Attachments/7426/MX25L3233F,%203V,%2032Mb,%20v1.6.pdf -#define MX25L3233F \ - { \ - .total_size = (1UL << 22), /* 4 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc2, \ - .memory_type = 0x20, .capacity = 0x16, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x40, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = true, .is_fram = false, \ - } - -// Settings for the Macronix MX25L6433F 8MiB SPI flash. -// Datasheet: -// https://www.macronix.com/Lists/Datasheet/Attachments/7408/MX25L6433F,%203V,%2064Mb,%20v1.6.pdf -#define MX25L6433F \ - { \ - .total_size = (1UL << 23), /* 8 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc2, \ - .memory_type = 0x20, .capacity = 0x17, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x40, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = true, .is_fram = false, \ - } - -// Settings for the Macronix MX25R6435F 8MiB SPI flash. -// Datasheet: -// http://www.macronix.com/Lists/Datasheet/Attachments/7428/MX25R6435F,%20Wide%20Range,%2064Mb,%20v1.4.pdf -// By default its in lower power mode which can only do 8mhz. In high power mode -// it can do 80mhz. -#define MX25R6435F \ - { \ - .total_size = (1UL << 23), /* 8 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc2, \ - .memory_type = 0x28, .capacity = 0x17, .max_clock_speed_mhz = 8, \ - .quad_enable_bit_mask = 0x40, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = true, .is_fram = false, \ - } - -// Settings for the Macronix MX25L12833F 16MiB SPI flash. -// Datasheet: -// https://www.macronix.com/Lists/Datasheet/Attachments/7408/MX25L12833F,%203V,%2064Mb,%20v1.6.pdf -#define MX25L12833F \ - { \ - .total_size = (1UL << 24), /* 8 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xc2, \ - .memory_type = 0x20, .capacity = 0x18, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x40, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = true, .is_fram = false, \ - } - -// Settings for the Cypress (was Spansion) S25FL064L 8MiB SPI flash. -// Datasheet: http://www.cypress.com/file/316661/download -#define S25FL064L \ - { \ - .total_size = (1UL << 23), /* 8 MiB */ \ - .start_up_time_us = 300, .manufacturer_id = 0x01, .memory_type = 0x60, \ - .capacity = 0x17, .max_clock_speed_mhz = 108, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Cypress (was Spansion) S25FL116K 2MiB SPI flash. -// Datasheet: http://www.cypress.com/file/196886/download -#define S25FL116K \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 10000, .manufacturer_id = 0x01, \ - .memory_type = 0x40, .capacity = 0x15, .max_clock_speed_mhz = 108, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Cypress (was Spansion) S25FL216K 2MiB SPI flash. -// Datasheet: http://www.cypress.com/file/197346/download -#define S25FL216K \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 10000, .manufacturer_id = 0x01, \ - .memory_type = 0x40, .capacity = 0x15, .max_clock_speed_mhz = 65, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = false, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q80DL 1MiB SPI flash. -// https://www.winbond.com/resource-files/w25q80dv%20dl_revh_10022015.pdf - -#define W25Q80DL \ - { \ - .total_size = (1UL << 20), /* 1 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x14, .max_clock_speed_mhz = 80, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q80DV 1MiB SPI flash. -// https://www.winbond.com/resource-files/w25q80dv%20dl_revh_10022015.pdf - -#define W25Q80DV \ - { \ - .total_size = (1UL << 20), /* 1 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x14, .max_clock_speed_mhz = 104, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q16FW 2MiB SPI flash. -// Datasheet: -// https://www.winbond.com/resource-files/w25q16fw%20revj%2005182017%20sfdp.pdf -#define W25Q16FW \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x60, .capacity = 0x15, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q16JV-IQ 2MiB SPI flash. Note that JV-IM has a -// different .memory_type (0x70) Datasheet: -// https://www.winbond.com/resource-files/w25q16jv%20spi%20revf%2005092017.pdf -#define W25Q16JV_IQ \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x15, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q16JV-IM 2MiB SPI flash. Note that JV-IQ has a -// different .memory_type (0x40) Datasheet: -// https://www.winbond.com/resource-files/w25q16jv%20spi%20revf%2005092017.pdf -#define W25Q16JV_IM \ - { \ - .total_size = (1UL << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x70, .capacity = 0x15, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - } - -// Settings for the Winbond W25Q32BV 4MiB SPI flash. -// Datasheet: -// https://www.winbond.com/resource-files/w25q32bv_revi_100413_wo_automotive.pdf -#define W25Q32BV \ - { \ - .total_size = (1UL << 22), /* 4 MiB */ \ - .start_up_time_us = 10000, .manufacturer_id = 0xef, \ - .memory_type = 0x60, .capacity = 0x16, .max_clock_speed_mhz = 104, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q32FV 4MiB SPI flash. -// Datasheet:http://www.winbond.com/resource-files/w25q32fv%20revj%2006032016.pdf?__locale=en -#define W25Q32FV \ - { \ - .total_size = (1UL << 22), /* 4 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x16, .max_clock_speed_mhz = 104, \ - .quad_enable_bit_mask = 0x00, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = false, \ - .supports_qspi_writes = false, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q32JV-IM 4MiB SPI flash. -// https://www.winbond.com/resource-files/w25q32jv%20revg%2003272018%20plus.pdf -#define W25Q32JV_IM \ - { \ - .total_size = (1UL << 22), /* 4 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x70, .capacity = 0x16, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - } - -// Settings for the Winbond W25Q32JV-IQ 4MiB SPI flash. Note that JV-IM has a -// different .memory_type (0x70) Datasheet: -// https://www.winbond.com/resource-files/w25q32jv%20revh%2001072019%20plus.pdf -#define W25Q32JV_IQ \ - { \ - .total_size = (1UL << 22), /* 4 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x16, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q64JV-IM 8MiB SPI flash. Note that JV-IQ has a -// different .memory_type (0x40) Datasheet: -// http://www.winbond.com/resource-files/w25q64jv%20revj%2003272018%20plus.pdf -#define W25Q64JV_IM \ - { \ - .total_size = (1UL << 23), /* 8 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x70, .capacity = 0x17, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q64JV-IQ 8MiB SPI flash. Note that JV-IM has a -// different .memory_type (0x70) Datasheet: -// http://www.winbond.com/resource-files/w25q64jv%20revj%2003272018%20plus.pdf -#define W25Q64JV_IQ \ - { \ - .total_size = (1UL << 23), /* 8 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x17, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q128JV-SQ 16MiB SPI flash. Note that JV-IM has a -// different .memory_type (0x70) Datasheet: -// https://www.winbond.com/resource-files/w25q128jv%20revf%2003272018%20plus.pdf -#define W25Q128JV_SQ \ - { \ - .total_size = (1UL << 24), /* 16 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x18, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q128JV-PM 16MiB SPI flash. Note that JV-IM has a -// different .memory_type (0x70) Datasheet: -// https://www.winbond.com/resource-files/w25q128jv%20revf%2003272018%20plus.pdf -#define W25Q128JV_PM \ - { \ - .total_size = (1UL << 24), /* 16 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x70, .capacity = 0x18, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Winbond W25Q256JV 32MiB SPI flash. -// https://www.winbond.com/resource-files/w25q256jv%20spi%20revg%2008032017.pdf -#define W25Q256JV \ - { \ - .total_size = (1UL << 25), /* 32 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0xef, \ - .memory_type = 0x40, .capacity = 0x19, .max_clock_speed_mhz = 133, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Zetta Device ZD25WQ16B 2MiB SPI flash. -// Datasheet: http://www.zettadevice.com/upload/file/pdf/ZD25WQ16B_datasheet.pdf -#define ZD25WQ16B \ - { \ - .total_size = (1 << 21), /* 2 MiB */ \ - .start_up_time_us = 12000, .manufacturer_id = 0xba, \ - .memory_type = 0x60, .capacity = 0x15, .max_clock_speed_mhz = 85, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -// Settings for the Puya Semiconductor P25Q16H 2MiB QSPI flash. -// Datasheet: -// https://www.puyasemi.com/uploadfiles/2021/12/202112201130233023.pdf -#define P25Q16H \ - { \ - .total_size = (1 << 21), /* 2 MiB */ \ - .start_up_time_us = 5000, .manufacturer_id = 0x85, \ - .memory_type = 0x60, .capacity = 0x15, .max_clock_speed_mhz = 104, \ - .quad_enable_bit_mask = 0x02, .has_sector_protection = false, \ - .supports_fast_read = true, .supports_qspi = true, \ - .supports_qspi_writes = true, .write_status_register_split = false, \ - .single_status_byte = false, .is_fram = false, \ - } - -#endif // MICROPY_INCLUDED_ATMEL_SAMD_EXTERNAL_FLASH_DEVICES_H diff --git a/src/madflight/bb/bb.h b/src/madflight/bb/bb.h index a890418a..ee47eab4 100644 --- a/src/madflight/bb/bb.h +++ b/src/madflight/bb/bb.h @@ -1,186 +1,92 @@ -/*======================================================================================================================== -This file contains all necessary functions and code used for blackbox logging to avoid cluttering the main code +/*========================================================================================== +BB: madflight black box data logger in ArduPilot Binary Log data format -Each USE_BB_xxx section in this file defines: - - bb.setup() - setup interface - - bb.csvDump() - dump last log to serial debug port as csv (tab delimited text) - - bb.erase() - erase files - - bb.dir() - list files +MIT License -In addition there are these functions common for all blackbox variants: - - bb.start() - start a logging session - - bb.stop() - stop a logging session - - bb.log_xxx() - call these functions to write to logger, modify/add functions as needed +Copyright (c) 2024 qqqlab - https://github.com/qqqlab -HOW TO ADD YOUR OWN LOGGERS - 1. add a recordtype_e enum - 2. add a new BlackBoxBase::log_xxx() for your logger. - 3. add a call to your logger in BlackBoxBase::start() to write the header - 4. call bb.log_xxx() in madflight.ino to log data +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: -Note: this header needs to be included after all globals are defined in madflight.ino, so that these variables are visible here without declaring each global variable extern in this file. -========================================================================================================================*/ +The above copyright notice and this permission notice shall be included in all +copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +SOFTWARE. +===========================================================================================*/ #define BB_USE_NONE 1 -#define BB_USE_INTFLASH 2 //internal QSPI/OSPI flash -#define BB_USE_FLASH 3 //external SPI flash -#define BB_USE_RAM 4 //internal RAM (or PSRAM on ESP32) -#define BB_USE_SDCARD 5 //SDCARD with MMC interface +#define BB_USE_SDCARD 2 //SDCARD with MMC interface +#define BB_USE_DEBUG 3 //print log to Serial -#include "BlackBox_Defines.h" -#include "BlackBoxWriter.h" -#include "BlackBoxDecoder.h" -class BlackBox { +//uncomment to use 32bit timestamp (good for 1 hour of recording before wrap-around) +#define LOG_TIMEUS_U32 -private: +#define LOG_TYPE_LEN 64 //max number of message types - enum recordtype_e { - BB_REC_IMU, - BB_REC_PID, - BB_REC_BAT, - BB_REC_BARO, - BB_REC_GPS, - BB_REC_MODE, - }; +#define HEAD_BYTE1 0xA3 // Decimal 163 +#define HEAD_BYTE2 0x95 // Decimal 149 +//black box file system +class BBFS { public: + virtual void setup() = 0; //setup the file system (can be called multiple times) + + virtual bool writeOpen() = 0; //create new file for writing (closes previously opened file first) + virtual void writeChar(uint8_t c) = 0; //write char to file + virtual void write(const uint8_t *buf, const uint8_t len) = 0; + virtual void close() = 0; //close file + + virtual void erase() = 0; //erase all files + virtual void dir() = 0; //list files + virtual void bench() = 0; //benchmark read/write +}; - //loggers - Use writeU() and writeI() to write variable byte encoded integer, which use 1-5 bytes - void log_imu() { - if(bbw.isBusy()) return; //sets busy flag - bbw.writeBeginRecord(BB_REC_IMU, "IMU"); - bbw.writeU("ts",micros()); - bbw.writeI("ax*100",ahrs.ax*100); //G - bbw.writeI("ay*100",ahrs.ay*100); //G - bbw.writeI("az*100",ahrs.az*100); //G - bbw.writeI("gx*10",ahrs.gx*10); //dps - bbw.writeI("gy*10",ahrs.gy*10); //dps - bbw.writeI("gz*10",ahrs.gz*10); //dps - bbw.writeI("mx*100",ahrs.mx*100); //uT - bbw.writeI("my*100",ahrs.my*100); //uT - bbw.writeI("mz*100",ahrs.mz*100); //uT - bbw.writeI("roll*10",ahrs.roll*10); //deg - bbw.writeI("pitch*10",ahrs.pitch*10);; //deg - bbw.writeI("yaw*10",ahrs.yaw*10);; //deg - bbw.writeEndrecord(); //clears busy flag - } - - void log_pid() { - if(bbw.isBusy()) return; - bbw.writeBeginRecord(BB_REC_PID, "PID"); - bbw.writeU("ts",micros()); - bbw.writeI("roll_PID*100",roll_PID*100); //-1 to +1 - bbw.writeI("pitch_PID*100",pitch_PID*100);; //-1 to +1 - bbw.writeI("yaw_PID*100",yaw_PID*100);; //-1 to +1 - bbw.writeEndrecord(); - } - - void log_bat() { - if(bbw.isBusy()) return; - bbw.writeBeginRecord(BB_REC_BAT, "BAT"); - bbw.writeU("ts",micros()); - bbw.writeU("bat_mA",bat.i*1000); //Battery current (A) - bbw.writeU("bat_mV",bat.v*1000); //battery voltage (V) - bbw.writeU("bat_mAh",bat.mah); //battery usage (Ah) - bbw.writeU("bat_mWh",bat.wh*1000); //battery usage (Wh) - bbw.writeEndrecord(); - } - - void log_baro() { - if(bbw.isBusy()) return; - bbw.writeBeginRecord(BB_REC_BARO, "BARO"); - bbw.writeU("ts",micros()); - bbw.writeU("baro_pa",baro.press); //Barometer pressure (Pa) - bbw.writeI("baro_t*100",baro.temp*100); //barometer temp (C) - bbw.writeEndrecord(); - } - - void log_gps() { - if(bbw.isBusy()) return; - bbw.writeBeginRecord(BB_REC_GPS, "GPS"); - bbw.writeU("ts",micros()); - bbw.writeI32("lat",gps.lat); - bbw.writeI32("lon",gps.lon); - bbw.writeEndrecord(); - } - - void log_mode(uint8_t mode = 0, const char* name = nullptr) { - if(bbw.isBusy()) return; - bbw.writeBeginRecord(BB_REC_MODE, "MODE"); - bbw.writeU("ts",micros()); - bbw.writeU("mode",mode); - //bbw.writeString("name",name); - bbw.writeEndrecord(); - } - - - - void start() { - if(isStarted) return; - isStarted = true; - //create a new file - fs->writeOpen(); - bbw.writeHeaders(); - //add here all loggers, this will write headers with record and field info which are needed by the decoder - log_imu(); - log_pid(); - log_bat(); - log_baro(); - log_gps(); - log_mode(); - //start logging - bbw.startLogging(); - } - - void stop() { - if(!isStarted) return; - isStarted = false; - bbw.stopLogging(); - fs->writeClose(); - } - - void setup() { - isStarted = false; - fs->setup(); - bbw.setup(fs); - } - - void csvDump(int fileno) { - fs->readOpen(fileno); - BlackBoxDecoder bbd; - bbd.csv_decode(fs); - } - - void erase() { - stop(); - fs->erase(); - } +//===================================================================================================================== +// Logging to Serial +//===================================================================================================================== +#if BB_USE == BB_USE_DEBUG - void dir() { - fs->dir(); +class BBFS_Debug : public BBFS { +public: + void setup() override {} + bool writeOpen() override {return true;} + + void writeChar(uint8_t c) override { + static int cnt=0; + Serial.printf("%02X ", c); + cnt++; + if(cnt==32) { + Serial.println(); + cnt=0; + } } - - - BlackBox(BlackBoxFS *fs) { - this->fs = fs; + void write(const uint8_t *buf, const uint8_t len) override { + for(int i=0;i= sizeof(buf_write)) { - file.write(buf_write, sizeof(buf_write)); + if(wbuf_len >= sizeof(wbuf)) { + file.write(wbuf, sizeof(wbuf)); file.flush(); - memset(buf_write, 0xff, sizeof(buf_write)); - buf_write_idx = 0; - //Serial.println("BB: write sector"); + memset(wbuf, 0xff, sizeof(wbuf)); + wbuf_len = 0; } } - void writeClose() { - if(buf_write_idx>0) { - file.write(buf_write, buf_write_idx); + void write(const uint8_t *buf, const uint8_t len) override { + for(int i=0;i0) { + file.write(wbuf, wbuf_len); + wbuf_len = 0; } file.close(); } - void readOpen(int fileno) { + void erase() override { + if(mmc_setup()) { + mmc_deleteFilesFromDir(fs, BB_LOG_DIR_NAME); + } } - uint8_t readChar() { - return 0xff; + void dir () override { + int attempt = 0; + while(++attempt<2) { + if(mmc_setup()) { + if(mmc_listDir(fs, BB_LOG_DIR_NAME, 0)) return; + } + setup_done = false; //force setup to re-run + } } - void erase() { - } - void dir() { - if(!setup_done) return; - mmc_listDir(fs, "/log", 0); + void bench() override { + const char* path = "/madfli.ght"; + uint8_t *buf; + size_t len = 0; + uint32_t start; + uint32_t end; + size_t flen; + File file; + + buf = (uint8_t*)malloc(512); + if(!buf) { + Serial.println("BB: bench - malloc failed"); + return; + } + + file = fs.open(path, FILE_WRITE); + if(!file){ + Serial.println("BB: bench - Failed to open file for writing"); + free(buf); + return; + } + + size_t i; + for(i=0;i<512;i++) buf[i] = i%64<63 ? '0' + i%64 - 1 : '\n'; + + start = millis(); + for(i=0; i<2048; i++){ + sprintf((char*)buf,"%u ",i); + file.write(buf, 512); + } + end = millis() - start; + flen = 2048 * 512; + Serial.printf("BB: %s %u bytes written in %u ms %f kbps\r\n", path, (int)flen, (int)end, (float)flen/end); + file.close(); + + file = fs.open(path); + if(!file){ + Serial.println("BB: bench - Failed to open file for reading"); + free(buf); + return; + } + len = file.size(); + flen = len; + start = millis(); + while(len){ + size_t toRead = len; + if(toRead > 512){ + toRead = 512; + } + file.read(buf, toRead); + len -= toRead; + } + end = millis() - start; + Serial.printf("BB: %s %u bytes read in %u ms %f kbps\r\n", path, (int)flen, (int)end, (float)flen/end); + file.close(); + + free(buf); } + private: bool mmc_setup() { + if(setup_done) return true; + + SD_MMC.end(); //force begin() to re-initialize MMC + SD_MMC.setPins(HW_PIN_SDMMC_CLK, HW_PIN_SDMMC_CMD, HW_PIN_SDMMC_DATA); if (!SD_MMC.begin("/sdcard", true, true, SDMMC_FREQ_DEFAULT, 5)) { Serial.println("Card Mount Failed"); - return false; + setup_done = false; + return setup_done; } uint8_t cardType = SD_MMC.cardType(); if(cardType == CARD_NONE){ - Serial.println("No SD_MMC card attached"); - return false; + Serial.println("No SD_MMC card attached"); + setup_done = false; + return setup_done; } Serial.print("CardType="); if(cardType == CARD_MMC){ @@ -279,26 +266,27 @@ class BlackBoxFS_SDMMC : public BlackBoxFS { Serial.print("UNKNOWN"); } - Serial.printf(" SectorSize=%d ", SD_MMC.sectorSize()); + Serial.printf(" SectorSize=%d", SD_MMC.sectorSize()); Serial.printf(" CardSize=%lluMB", SD_MMC.cardSize() / (1024 * 1024)); Serial.printf(" used:%lluMB", SD_MMC.usedBytes()/ (1024 * 1024)); Serial.printf(" free:%lluMB", (SD_MMC.totalBytes() - SD_MMC.usedBytes()) / (1024 * 1024)); Serial.println(); - return true; + setup_done = true; + return setup_done; } - void mmc_listDir(fs::FS &fs, const char * dirname, uint8_t levels){ + bool mmc_listDir(fs::FS &fs, const char * dirname, uint8_t levels){ Serial.printf("Listing directory: %s\n", dirname); File root = fs.open(dirname); if(!root){ Serial.println("Failed to open directory"); - return; + return false; } if(!root.isDirectory()){ Serial.println("Not a directory"); - return; + return true; } File file = root.openNextFile(); @@ -319,6 +307,21 @@ class BlackBoxFS_SDMMC : public BlackBoxFS { Serial.printf("SDCARD: used:%lluMB", SD_MMC.usedBytes()/ (1024 * 1024)); Serial.printf(" free:%lluMB\n", (SD_MMC.totalBytes() - SD_MMC.usedBytes()) / (1024 * 1024)); + return true; + } + + void mmc_deleteFilesFromDir(fs::FS &fs, const char * dirname){ + File root = fs.open(dirname); + if(!root) return; + if(!root.isDirectory()) return; + + File file = root.openNextFile(); + while(file){ + if(!file.isDirectory()){ + fs.remove(file.path()); + } + file = root.openNextFile(); + } } void mmc_logOpen(fs::FS &fs, const char * dirname){ @@ -335,7 +338,7 @@ class BlackBoxFS_SDMMC : public BlackBoxFS { } } if(!root.isDirectory()){ - Serial.println("BB: /log Not a directory"); + Serial.println("BB: /log is not a directory"); return; } @@ -353,329 +356,463 @@ class BlackBoxFS_SDMMC : public BlackBoxFS { Serial.println(filename); file = fs.open(filename, FILE_WRITE, true); } - }; -BlackBoxFS_SDMMC bb_fs; - +BBFS_SDMMC bbfs; //===================================================================================================================== -// Logging to FLASH (internal or external) +// No Logging //===================================================================================================================== -#elif BB_USE == BB_USE_INTFLASH || BB_USE == BB_USE_FLASH - -#include "SPIFlash/Adafruit_SPIFlashBase.h" - -#if BB_USE == BB_USE_FLASH - Adafruit_FlashTransport_SPI flashTransport(HW_PIN_BB_CS, bb_spi); -#elif BB_USE == BB_USE_INTFLASH - #if defined(ARDUINO_ARCH_ESP32) - // ESP32 use same flash device that store code for file system. - // SPIFlash will parse partition.cvs to detect FATFS/SPIFS partition to use - Adafruit_FlashTransport_ESP32 flashTransport; - #elif defined(ARDUINO_ARCH_RP2040) - // RP2040 use same flash device that store code for file system. Therefore we - // only need to specify start address and size (no need SPI or SS) - // By default (start=0, size=0), values that match file system setting in - // 'Tools->Flash Size' menu selection will be used. - Adafruit_FlashTransport_RP2040 flashTransport; - #else - #error BB_USE_INTFLASH No internal flash defined for your board ! - #endif -#elif - #error Missing define BB_USE_FLASH or BB_USE_INTFLASH -#endif +#else -Adafruit_SPIFlashBase flash(&flashTransport); +class BBFS_None : public BBFS { +public: + void setup() override {} + bool writeOpen() override {return false;} + void writeChar(uint8_t c) override {} + void write(const uint8_t *buf, const uint8_t len) override {} + void close() override {} + void erase() override {} + void dir() override {} + void bench() override {} +}; +BBFS_None bbfs; +#endif -class BlackBoxFS_Flash : public BlackBoxFS { -private: - uint32_t flash_write_addr; - uint8_t buf_write[256]; - int buf_write_idx; - uint32_t flash_read_addr; - uint8_t buf_read[256]; - int buf_read_idx; +class BinLog { + //------------------------------- + //locking handling + //------------------------------- +private: + static bool locked; public: - void setup() { - flash.begin(); - flash_write_addr = findStartPage(); - Serial.printf("BB_USE_FLASH JEDEC=%X size=%d free=%d\n", flash.getJEDECID(), flash.size(), flash.size()-flash_write_addr); - } - - void writeOpen() { - memset(buf_write, 0xff, 256); - buf_write_idx = 0; - flash_write_addr = findStartPage(); + static bool getLock() { + if(!started) return false; + if(locked) return false; + locked = true; + return true; } - void writeChar(uint8_t c) { - if(buf_write_idx < 256) { - buf_write[buf_write_idx++] = c; - } - if(buf_write_idx >= 256) { - flash.writeBuffer(flash_write_addr, buf_write, 256); - flash_write_addr += 256; - memset(buf_write, 0xff, 256); - buf_write_idx = 0; + //------------------------------- + //FMT handling + //------------------------------- +private: + struct PACKED { + uint8_t h1 = HEAD_BYTE1; + uint8_t h2 = HEAD_BYTE2; + uint8_t type = 0x80; + uint8_t msg_type; + uint8_t length; + char name[4]; + char format[16]; + char labels[64]; + } FMT; + uint8_t FMT_fmt; //format position + uint8_t FMT_lbl; //label position + bool FMT_write = false; + + + static void FMT_sendFMT() { + struct PACKED { + uint8_t h1 = HEAD_BYTE1; + uint8_t h2 = HEAD_BYTE2; + uint8_t type = 0x80; + uint8_t msg_type = 0x80; + uint8_t length = 0x59; + char name[4] = "FMT"; + char format[16] = "BBnNZ"; + char labels[64] = "Type,Length,Name,Format,Columns"; + } FMT; + bbfs.write((uint8_t*)&FMT, sizeof(FMT)); +/* + const uint8_t msg_fmt[] = { + 0xA3, 0x95, 0x80, 0x80, 0x59, 0x46, 0x4D, 0x54, 0x00, 0x42, 0x42, 0x6E, 0x4E, 0x5A, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x54, 0x79, 0x70, 0x65, 0x2C, 0x4C, 0x65, 0x6E, 0x67, 0x74, 0x68, 0x2C, 0x4E, 0x61, 0x6D, 0x65, 0x2C, 0x46, 0x6F, 0x72, 0x6D, 0x61, 0x74, 0x2C, 0x43, 0x6F, 0x6C, 0x75, 0x6D, 0x6E, 0x73, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00}; + bbfs.write(msg_fmt, sizeof(msg_fmt)); +*/ + } + + void FMT_addField(const char fmt, const char* label) { + if (FMT_fmt >= 16) { + FMT.format[0]=0; + Serial.printf("BB: ERROR too many fields for msg %s\n", FMT.name); + return; } - } + FMT.format[FMT_fmt++] = fmt; - void writeClose() { - if(buf_write_idx==0) return; - //write 0xff in unused part of page - memset(buf_write + buf_write_idx, 0xff, 256 - buf_write_idx); - //write final page - flash.writeBuffer(flash_write_addr, buf_write, 256); - flash_write_addr += 256; - } - - void readOpen(int fileno) { - int adr[100]; - int filecnt = findFiles(adr,100); - if(fileno <= 0) { - fileno = filecnt - 1 + fileno; //0:last file, -1:2nd last file, etc - }else{ - fileno--; + int lbl_len = strlen(label); + if (FMT_lbl + 1 + lbl_len > 64) { + FMT.format[0]=0; + Serial.printf("BB: ERROR labels too long for msg %s\n", FMT.name); + return; } - if(fileno<0 || fileno>=filecnt) fileno = filecnt; - readSeek( adr[fileno] ); + if (FMT_lbl) FMT.labels[FMT_lbl++] = ','; + strcpy(FMT.labels+FMT_lbl, label); + FMT_lbl += lbl_len; } - uint8_t readChar() { - if(buf_read_idx >= 256) { - flash.readBuffer(flash_read_addr, buf_read, 256); - flash_read_addr += 256; - buf_read_idx = 0; + //------------------------------- + //message type handling + //------------------------------- +private: + static uint32_t msg_name[LOG_TYPE_LEN]; + static uint8_t msg_name_len; + + uint8_t find_msg_type(const char *name) { + uint8_t i; + uint32_t findname = 0; + strncpy((char*)&findname,name,4); + for(i=0;i which drives the translaton of flightmode codes to names (among other things probably) + //_log_msg("ArduCopter"); //gives problems with plot.ardupilot.org + _log_msg(MADFLIGHT_VERSION); + _log_parm("DUMMY",0,0); //keep plot.ardupilot.org happy + + //allow logging + locked = false; + started = true; } + + static void stop() { + //stop logging + started = false; - void dir() { - int adr[100]; - int filecnt = findFiles(adr,100); - for(int i=0;i=filecnt) fileno = filecnt; - buf_r = adr[fileno]; } - //read char from file - uint8_t readChar() { - if(buf_r < buf_w) { - return buf[buf_r++]; - }else{ - return 0xff; + void msg_end() { + if(FMT_write) { + FMT.length = buflen; + bbfs.write((uint8_t*)&FMT, sizeof(FMT)); + FMT_write = false; } + bbfs.write(buf,buflen); + buflen = 0; + locked = false; } - //erase all files - void erase() { - setup(); +public: + void TimeUS(uint64_t v) { + #ifdef LOG_TIMEUS_U32 + u32("TimeUS",v); + #else + u64("TimeUS",v); + #endif } - //list files - void dir() { - int adr[100]; - int filecnt = findFiles(adr,100); - for(int i=0;i