diff --git a/Makefile b/Makefile index a092d8d4e..0f1945e41 100644 --- a/Makefile +++ b/Makefile @@ -294,6 +294,8 @@ NAZE_SRC = startup_stm32f10x_md_gcc.S \ drivers/system_stm32f10x.c \ drivers/timer.c \ drivers/timer_stm32f10x.c \ + drivers/flash_m25p16.c \ + io/flashfs.c \ hardware_revision.c \ $(HIGHEND_SRC) \ $(COMMON_SRC) diff --git a/docs/Blackbox.md b/docs/Blackbox.md index 05effe956..e7f4b4f1d 100644 --- a/docs/Blackbox.md +++ b/docs/Blackbox.md @@ -5,7 +5,7 @@ ## Introduction This feature transmits your flight data information on every control loop iteration over a serial port to an external -logging device to be recorded. +logging device to be recorded, or to a dataflash chip which is present on some flight controllers. After your flight, you can process the resulting logs on your computer to either turn them into CSV (comma-separated values) or render your flight log as a video using the tools `blackbox_decode` and `blackbox_render`. Those tools can be @@ -21,20 +21,17 @@ https://github.com/cleanflight/blackbox-log-viewer The blackbox records flight data on every iteration of the flight control loop. It records the current time in microseconds, P, I and D corrections for each axis, your RC command stick positions (after applying expo curves), gyroscope data, accelerometer data (after your configured low-pass filtering), barometer readings, 3-axis magnetometer -readings, raw VBAT measurements, and the command being sent to each motor speed controller. This is all stored without -any approximation or loss of precision, so even quite subtle problems should be detectable from the fight data log. +readings, raw VBAT and current measurements, and the command being sent to each motor speed controller. This is all +stored without any approximation or loss of precision, so even quite subtle problems should be detectable from the fight +data log. -Currently, the blackbox attempts to log GPS data whenever new GPS data is available, but this has not been tested yet. -The CSV decoder and video renderer do not yet show any of the GPS data (though this will be added). If you have a working -GPS, please send in your logs so I can get the decoding implemented. - -The data rate for my quadcopter using a looptime of 2400 is about 10.25kB/s. This allows about 18 days of flight logs -to fit on a 16GB MicroSD card, which ought to be enough for anybody :). +GPS data is logged whenever new GPS data is available. Although the CSV decoder will decode this data, the video +renderer does not yet show any of the GPS information (this will be added later). ## Supported configurations -The maximum data rate for the flight log is fairly restricted, so anything that increases the load can cause the flight -log to drop frames and contain errors. +The maximum data rate that can be recorded to the flight log is fairly restricted, so anything that increases the load +can cause the flight log to drop frames and contain errors. The Blackbox was developed and tested on a quadcopter. It has also been tested on a tricopter. It should work on hexacopters or octocopters, but as they transmit more information to the flight log (due to having more motors), the @@ -49,10 +46,11 @@ details. ## Hardware -The blackbox software is designed to be used with an [OpenLog serial data logger][] and a microSDHC card. You need a -little prep to get the OpenLog ready for use, so here are the details: +There are two options for storing your flight logs. You can either transmit the log data over a serial port to an +external logging device like the [OpenLog serial data logger][] to be recorded to a microSDHC card, or if you have a +compatible flight controller you can store the logs on the onboard dataflash storage instead. -### Firmware +### OpenLog serial data logger The OpenLog ships with standard OpenLog 3 firmware installed. However, in order to reduce the number of dropped frames, it should be reflashed with the [OpenLog Light firmware][] or the special [OpenLog Blackbox firmware][] . The Blackbox @@ -66,18 +64,18 @@ along with instructions for installing it onto your OpenLog. [OpenLog Light firmware]: https://github.com/sparkfun/OpenLog/tree/master/firmware/OpenLog_v3_Light [OpenLog Blackbox firmware]: https://github.com/cleanflight/blackbox-firmware -### microSDHC +#### microSDHC Your choice of microSDHC card is very important to the performance of the system. The OpenLog relies on being able to make many small writes to the card with minimal delay, which not every card is good at. A faster SD-card speed rating is not a guarantee of better performance. -#### microSDHC cards known to have poor performance +##### microSDHC cards known to have poor performance - Generic 4GB Class 4 microSDHC card - the rate of missing frames is about 1%, and is concentrated around the most interesting parts of the log! -#### microSDHC cards known to have good performance +##### microSDHC cards known to have good performance - Transcend 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%) - Sandisk Extreme 16GB Class 10 UHS-I microSDHC (typical error rate < 0.1%) @@ -87,7 +85,7 @@ the best chance of writing at high speed. You must format it with either FAT, or [SD Association's special formatting tool]: https://www.sdcard.org/downloads/formatter_4/ -### OpenLog configuration +#### OpenLog configuration This section applies only if you are using the OpenLog or OpenLog Light original firmware on the OpenLog. If you flashed it with the special OpenLog Blackbox firmware, you don't need to configure it further. @@ -105,42 +103,16 @@ of the MicroSD card: baud,escape,esc#,mode,verb,echo,ignoreRX ``` -## Enabling this feature (CLI) +#### Serial port -Enable the Blackbox feature by typing in `feature BLACKBOX` and pressing enter. You also need to assign the Blackbox to -one of [your serial ports][] . A 115200 baud port is required (such as serial_port_1 on the Naze32, the two-pin Tx/Rx -header in the center of the board). - -For example, use `set serial_port_1_scenario=11` to switch the main serial port to MSP, CLI, Blackbox and GPS Passthrough. - -Enter `save`. Your configuration should be saved and the flight controller will reboot. You're ready to go! - -[your serial ports]: https://github.com/cleanflight/cleanflight/blob/master/docs/Serial.md -[Cleanflight Configurator]: https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb?hl=en - -## Configuring this feature - -The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control -the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which -decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs -every iteration. - -If you are using a short looptime like 2000 or faster, or you're using a slower MicroSD card, you will need to reduce -this rate to reduce the number of corrupted logged frames. A rate of 1/2 is likely to work for most craft. - -You can change these settings by entering the CLI tab in the Cleanflight Configurator and using the `set` command, like so: - -``` -set blackbox_rate_num = 1 -set blackbox_rate_denom = 2 -``` - -### Serial port +A 115200 baud serial port is required to connect the OpenLog to your flight controller (such as `serial_port_1` on the +Naze32, the two-pin Tx/Rx header in the center of the board). The Blackbox can not be used with softserial ports as they +are too slow. Connect the "TX" pin of the serial port you've chosen to the OpenLog's "RXI" pin. Don't connect the serial port's RX pin to the OpenLog. -### Protection +#### Protection The OpenLog can be wrapped in black electrical tape or heat-shrink in order to insulate it from conductive frames (like carbon fiber), but this makes its status LEDs impossible to see. I recommend wrapping it with some clear heatshrink @@ -148,30 +120,93 @@ tubing instead. ![OpenLog installed](Wiring/blackbox-installation-1.jpg "OpenLog installed with double-sided tape, SDCard slot pointing outward") +### Onboard dataflash storage +The full version of the Naze32 has an onboard "m25p16" 2 megayte dataflash storage chip which can be used to store +flight logs instead of using an OpenLog. This is the small chip at the base of the Naze32's direction arrow. +This chip is not present on the "Acro" version of the Naze32. + +## Enabling the Blackbox (CLI) +In the [Cleanflight Configurator][] , enter the CLI tab. Enable the Blackbox feature by typing in `feature BLACKBOX` and +pressing enter. Now choose the device that you want to log to: + +### OpenLog serial data logger +Enter `set blackbox_device=0` to switch to logging to a serial port (this is the default). + +You then need to let Cleanflight know which of [your serial ports][] you connected the OpenLog to. A 115200 baud port +is required (such as `serial_port_1` on the Naze32, the two-pin Tx/Rx header in the center of the board). + +For example, use `set serial_port_1_scenario=11` to switch the main serial port to MSP, CLI, Blackbox and GPS +Passthrough. You can also use the GUI to configure a port for the Blackbox feature on the Ports tab. + +### Onboard dataflash +Enter `set blackbox_device=1` to switch to logging to an onboard dataflash chip, if your flight controller has one. + +[your serial ports]: https://github.com/cleanflight/cleanflight/blob/master/docs/Serial.md +[Cleanflight Configurator]: https://chrome.google.com/webstore/detail/cleanflight-configurator/enacoimjcgeinfnnnpajinjgmkahmfgb?hl=en + +## Configuring the Blackbox + +The Blackbox currently provides two settings (`blackbox_rate_num` and `blackbox_rate_denom`) that allow you to control +the rate at which data is logged. These two together form a fraction (`blackbox_rate_num / blackbox_rate_denom`) which +decides what portion of the flight controller's control loop iterations should be logged. The default is 1/1 which logs +every iteration. + +If you are using a short looptime like 2000 or faster, or you're using a slower MicroSD card, you will need to reduce +this rate to reduce the number of corrupted logged frames that `blackbox_decode` complains about. A rate of 1/2 is likely +to work for most craft. + +You can change these settings by entering the CLI tab in the [Cleanflight Configurator][] and using the `set` command, like so: + +``` +set blackbox_rate_num = 1 +set blackbox_rate_denom = 2 +``` + +The data rate for my quadcopter using a looptime of 2400 and a rate of 1/1 is about 10.25kB/s. This allows about 18 +days of flight logs to fit on my OpenLog's 16GB MicroSD card, which ought to be enough for anybody :). + +If you're logging to an onboard dataflash chip instead of an OpenLog, be aware that the 2MB of storage space it offers +is pretty small. At the default 1/1 logging rate, and a 2400 looptime, this is only enough for about 3 minutes of +flight. This could be long enough for you to investigate some flying problem with your craft, but you may want to reduce +the logging rate in order to extend your recording time. + +To maximize your recording time, you could drop the rate way down to 1/32 (the smallest possible rate) which would +result in a logging rate of about 10-20Hz and about 650 bytes/second of data. At that logging rate, the 2MB flash chip +can store around 50 minutes of flight data, though the level of detail is severely reduced and you could not diagnose +flight problems like vibration or PID setting issues. + ## Usage -The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. Each time the OpenLog is -power-cycled, it begins a fresh new log file. If you arm and disarm several times without cycling the power (recording -several flights), those logs will be combined together into one file. The command line tools will ask you to pick which -one of these flights you want to display/decode. +The Blackbox starts recording data as soon as you arm your craft, and stops when you disarm. -If your craft has a buzzer attached, a short beep will be played when you arm. You can later use this beep to -synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line in the flight -data log, which you can sync against the beep in your recorded audio track). +If your craft has a buzzer attached, a short beep will be played when you arm and recording begins. You can later use +this beep to synchronize your recorded flight video with the rendered flight data log (the beep is shown as a blue line +in the flight data log, which you can sync against the beep in your recorded audio track). -The OpenLog requires a couple of seconds of delay after connecting the battery before it's ready to record, so don't -arm your craft immediately after connecting the battery (you'll probably be waiting for the flight controller to become -ready during that time anyway!) +You should wait a few seconds after disarming your craft to allow the Blackbox to finish saving its data. -You should also wait a few seconds after disarming the quad to allow the OpenLog to finish saving its data. +### Usage - OpenLog +Each time the OpenLog is power-cycled, it begins a fresh new log file. If you arm and disarm several times without +cycling the power (recording several flights), those logs will be combined together into one file. The command line +tools will ask you to pick which one of these flights you want to display/decode. Don't insert or remove the SD card while the OpenLog is powered up. +### Usage - Dataflash chip +After your flights, you can use the [Cleanflight Configurator][] to download the contents of the dataflash to your +computer. Go to the "dataflash" tab and click the "save flash to file..." button. Saving the log can take 2 or 3 +minutes. + +![Dataflash tab in Configurator](Screenshots/blackbox-dataflash.png) + +After downloading the log, be sure to erase the chip to make it ready for reuse by clicking the "erase flash" button. +If you try to start recording a new flight when the dataflash is already full, the Blackbox will not make its regular +arming beep and nothing will be recorded. + ## Converting logs to CSV or PNG -After your flights, you'll have a series of files labeled "LOG00001.TXT" etc. on the microSD card. You'll need to -decode these with the `blackbox_decode` tool to create a CSV (comma-separated values) file for analysis, or render them -into a series of PNG frames with `blackbox_render` tool, which you could then convert into a video using another -software package. +After your flights, you'll have a series of flight log files with a .TXT extension. You'll need to decode these with +the `blackbox_decode` tool to create CSV (comma-separated values) files for analysis, or render them into a series of PNG +frames with `blackbox_render` tool, which you could then convert into a video using another software package. You'll find those tools along with instructions for using them in this repository: diff --git a/docs/Screenshots/blackbox-dataflash.png b/docs/Screenshots/blackbox-dataflash.png new file mode 100644 index 000000000..1f891c0c5 Binary files /dev/null and b/docs/Screenshots/blackbox-dataflash.png differ diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 4bd7c9498..0ee123476 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -609,6 +609,10 @@ static void validateBlackboxConfig() masterConfig.blackbox_rate_num /= div; masterConfig.blackbox_rate_denom /= div; } + + if (masterConfig.blackbox_device >= BLACKBOX_DEVICE_END) { + masterConfig.blackbox_device = BLACKBOX_DEVICE_SERIAL; + } } void startBlackbox(void) @@ -642,6 +646,9 @@ void startBlackbox(void) } } +/** + * Begin Blackbox shutdown. + */ void finishBlackbox(void) { if (blackboxState == BLACKBOX_STATE_RUNNING) { @@ -791,7 +798,8 @@ static bool sendFieldDefinition(const char * const *headerNames, unsigned int he charsWritten = blackboxPrint("H Field "); charsWritten += blackboxPrint(headerNames[xmitState.headerIndex]); - charsWritten += blackboxPrint(":"); + blackboxWrite(':'); + charsWritten++; xmitState.u.fieldIndex++; needComma = false; @@ -1122,7 +1130,7 @@ void handleBlackbox(void) * * Don't wait longer than it could possibly take if something funky happens. */ - if (millis() > xmitState.u.startTime + 200 || isBlackboxDeviceIdle()) { + if (millis() > xmitState.u.startTime + 200 || blackboxDeviceFlush()) { blackboxDeviceClose(); blackboxSetState(BLACKBOX_STATE_STOPPED); } @@ -1130,6 +1138,11 @@ void handleBlackbox(void) default: break; } + + // Did we run out of room on the device? Stop! + if (isBlackboxDeviceFull()) { + blackboxSetState(BLACKBOX_STATE_STOPPED); + } } static bool canUseBlackboxWithCurrentConfiguration(void) diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 50226ff20..acaf0f4f8 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -1,5 +1,6 @@ #include #include +#include #include "blackbox_io.h" @@ -56,6 +57,8 @@ #include "config/config_profile.h" #include "config/config_master.h" +#include "io/flashfs.h" + #define BLACKBOX_BAUDRATE 115200 #define BLACKBOX_INITIAL_PORT_MODE MODE_TX @@ -68,7 +71,17 @@ static uint32_t previousBaudRate; void blackboxWrite(uint8_t value) { - serialWrite(blackboxPort, value); + switch (masterConfig.blackbox_device) { +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + flashfsWriteByte(value); // Write byte asynchronously + break; +#endif + case BLACKBOX_DEVICE_SERIAL: + default: + serialWrite(blackboxPort, value); + break; + } } static void _putc(void *p, char c) @@ -89,14 +102,31 @@ void blackboxPrintf(char *fmt, ...) // Print the null-terminated string 's' to the serial port and return the number of bytes written int blackboxPrint(const char *s) { - const char *pos = s; + int length; + const uint8_t *pos; - while (*pos) { - blackboxWrite(*pos); - pos++; + switch (masterConfig.blackbox_device) { + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + length = strlen(s); + flashfsWrite((const uint8_t*) s, length, false); // Write asynchronously + break; +#endif + + case BLACKBOX_DEVICE_SERIAL: + default: + pos = (uint8_t*) s; + while (*pos) { + serialWrite(blackboxPort, *pos); + pos++; + } + + length = pos - (uint8_t*) s; + break; } - return pos - s; + return length; } /** @@ -375,11 +405,25 @@ void blackboxWriteTag8_8SVB(int32_t *values, int valueCount) } /** - * If there is data waiting to be written to the blackbox device, attempt to write that now. + * If there is data waiting to be written to the blackbox device, attempt to write (a portion of) that now. + * + * Returns true if all data has been flushed to the device. */ -void blackboxDeviceFlush(void) +bool blackboxDeviceFlush(void) { - //Presently a no-op on serial + switch (masterConfig.blackbox_device) { + case BLACKBOX_DEVICE_SERIAL: + //Nothing to speed up flushing on serial, as serial is continuously being drained out of its buffer + return isSerialTransmitBufferEmpty(blackboxPort); + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + return flashfsFlushAsync(); +#endif + + default: + return false; + } } /** @@ -387,23 +431,6 @@ void blackboxDeviceFlush(void) */ bool blackboxDeviceOpen(void) { - blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - - serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); - serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); - beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); - } else { - blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); - - if (blackboxPort) { - previousPortMode = blackboxPort->mode; - previousBaudRate = blackboxPort->baudRate; - } - } - /* * We want to write at about 7200 bytes per second to give the OpenLog a good chance to save to disk. If * about looptime microseconds elapse between our writes, this is the budget of how many bytes we should @@ -413,26 +440,76 @@ bool blackboxDeviceOpen(void) */ blackboxWriteChunkSize = MAX((masterConfig.looptime * 9) / 1250, 4); - return blackboxPort != NULL; -} + switch (masterConfig.blackbox_device) { + case BLACKBOX_DEVICE_SERIAL: + blackboxPort = findOpenSerialPort(FUNCTION_BLACKBOX); + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; -void blackboxDeviceClose(void) -{ - serialSetMode(blackboxPort, previousPortMode); - serialSetBaudRate(blackboxPort, previousBaudRate); + serialSetBaudRate(blackboxPort, BLACKBOX_BAUDRATE); + serialSetMode(blackboxPort, BLACKBOX_INITIAL_PORT_MODE); + beginSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + } else { + blackboxPort = openSerialPort(FUNCTION_BLACKBOX, NULL, BLACKBOX_BAUDRATE, BLACKBOX_INITIAL_PORT_MODE, SERIAL_NOT_INVERTED); - endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + if (blackboxPort) { + previousPortMode = blackboxPort->mode; + previousBaudRate = blackboxPort->baudRate; + } + } - /* - * Normally this would be handled by mw.c, but since we take an unknown amount - * of time to shut down asynchronously, we're the only ones that know when to call it. - */ - if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { - mspAllocateSerialPorts(&masterConfig.serialConfig); + return blackboxPort != NULL; + break; +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + if (flashfsGetSize() == 0 || isBlackboxDeviceFull()) { + return false; + } + + return true; + break; +#endif + default: + return false; } } -bool isBlackboxDeviceIdle(void) +/** + * Close the Blackbox logging device immediately without attempting to flush any remaining data. + */ +void blackboxDeviceClose(void) { - return isSerialTransmitBufferEmpty(blackboxPort); + switch (masterConfig.blackbox_device) { + case BLACKBOX_DEVICE_SERIAL: + serialSetMode(blackboxPort, previousPortMode); + serialSetBaudRate(blackboxPort, previousBaudRate); + + endSerialPortFunction(blackboxPort, FUNCTION_BLACKBOX); + + /* + * Normally this would be handled by mw.c, but since we take an unknown amount + * of time to shut down asynchronously, we're the only ones that know when to call it. + */ + if (isSerialPortFunctionShared(FUNCTION_BLACKBOX, FUNCTION_MSP)) { + mspAllocateSerialPorts(&masterConfig.serialConfig); + } + break; + } +} + +bool isBlackboxDeviceFull(void) +{ + switch (masterConfig.blackbox_device) { + case BLACKBOX_DEVICE_SERIAL: + return false; + +#ifdef USE_FLASHFS + case BLACKBOX_DEVICE_FLASH: + return flashfsIsEOF(); +#endif + + default: + return false; + } } diff --git a/src/main/blackbox/blackbox_io.h b/src/main/blackbox/blackbox_io.h index 5abacecfa..62d590998 100644 --- a/src/main/blackbox/blackbox_io.h +++ b/src/main/blackbox/blackbox_io.h @@ -20,6 +20,18 @@ #include #include +#include "target.h" + +typedef enum BlackboxDevice { + BLACKBOX_DEVICE_SERIAL = 0, + +#ifdef USE_FLASHFS + BLACKBOX_DEVICE_FLASH, +#endif + + BLACKBOX_DEVICE_END +} BlackboxDevice; + uint8_t blackboxWriteChunkSize; void blackboxWrite(uint8_t value); @@ -34,8 +46,8 @@ void blackboxWriteTag2_3S32(int32_t *values); void blackboxWriteTag8_4S16(int32_t *values); void blackboxWriteTag8_8SVB(int32_t *values, int valueCount); -void blackboxDeviceFlush(void); +bool blackboxDeviceFlush(void); bool blackboxDeviceOpen(void); void blackboxDeviceClose(void); -bool isBlackboxDeviceIdle(void); +bool isBlackboxDeviceFull(void); diff --git a/src/main/config/config.c b/src/main/config/config.c index 0f0181d2e..b489ff3bd 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -478,6 +478,7 @@ static void resetConf(void) #endif #ifdef BLACKBOX + masterConfig.blackbox_device = 0; masterConfig.blackbox_rate_num = 1; masterConfig.blackbox_rate_denom = 1; #endif diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index b99770729..79a093cf4 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -88,6 +88,7 @@ typedef struct master_t { #ifdef BLACKBOX uint8_t blackbox_rate_num; uint8_t blackbox_rate_denom; + uint8_t blackbox_device; #endif uint8_t magic_ef; // magic number, should be 0xEF diff --git a/src/main/drivers/bus_spi.c b/src/main/drivers/bus_spi.c index 28fece232..248e17d39 100644 --- a/src/main/drivers/bus_spi.c +++ b/src/main/drivers/bus_spi.c @@ -198,7 +198,7 @@ uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t data) #endif } -bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len) +bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len) { uint16_t spiTimeout = 1000; diff --git a/src/main/drivers/bus_spi.h b/src/main/drivers/bus_spi.h index fd94abe09..62b20aeb0 100644 --- a/src/main/drivers/bus_spi.h +++ b/src/main/drivers/bus_spi.h @@ -24,7 +24,7 @@ bool spiInit(SPI_TypeDef *instance); void spiSetDivisor(SPI_TypeDef *instance, uint16_t divisor); uint8_t spiTransferByte(SPI_TypeDef *instance, uint8_t in); -bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, uint8_t *in, int len); +bool spiTransfer(SPI_TypeDef *instance, uint8_t *out, const uint8_t *in, int len); uint16_t spiGetErrorCounter(SPI_TypeDef *instance); void spiResetErrorCounter(SPI_TypeDef *instance); diff --git a/src/main/drivers/flash.h b/src/main/drivers/flash.h new file mode 100644 index 000000000..6ccba0d7b --- /dev/null +++ b/src/main/drivers/flash.h @@ -0,0 +1,31 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +typedef struct flashGeometry_t { + uint8_t sectors; + + uint16_t pagesPerSector; + uint16_t pageSize; + + uint32_t sectorSize; + + uint32_t totalSize; +} flashGeometry_t; diff --git a/src/main/drivers/flash_m25p16.c b/src/main/drivers/flash_m25p16.c new file mode 100644 index 000000000..a6bd7b22f --- /dev/null +++ b/src/main/drivers/flash_m25p16.c @@ -0,0 +1,289 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "drivers/flash_m25p16.h" +#include "drivers/bus_spi.h" +#include "drivers/system.h" + +#define M25P16_INSTRUCTION_RDID 0x9F +#define M25P16_INSTRUCTION_READ_BYTES 0x03 +#define M25P16_INSTRUCTION_READ_STATUS_REG 0x05 +#define M25P16_INSTRUCTION_WRITE_STATUS_REG 0x01 +#define M25P16_INSTRUCTION_WRITE_ENABLE 0x06 +#define M25P16_INSTRUCTION_WRITE_DISABLE 0x04 +#define M25P16_INSTRUCTION_PAGE_PROGRAM 0x02 +#define M25P16_INSTRUCTION_SECTOR_ERASE 0xD8 +#define M25P16_INSTRUCTION_BULK_ERASE 0xC7 + +#define M25P16_STATUS_FLAG_WRITE_IN_PROGRESS 0x01 +#define M25P16_STATUS_FLAG_WRITE_ENABLED 0x02 + +#define DISABLE_M25P16 GPIO_SetBits(M25P16_CS_GPIO, M25P16_CS_PIN) +#define ENABLE_M25P16 GPIO_ResetBits(M25P16_CS_GPIO, M25P16_CS_PIN) + +// The timeout we expect between being able to issue page program instructions +#define DEFAULT_TIMEOUT_MILLIS 6 + +// These take sooooo long: +#define SECTOR_ERASE_TIMEOUT_MILLIS 5000 +#define BULK_ERASE_TIMEOUT_MILLIS 21000 + +static flashGeometry_t geometry; + +/* + * Whether we've performed an action that could have made the device busy for writes. + * + * This allows us to avoid polling for writable status when it is definitely ready already. + */ +static bool couldBeBusy = false; + +/** + * Send the given command byte to the device. + */ +static void m25p16_performOneByteCommand(uint8_t command) +{ + ENABLE_M25P16; + + spiTransferByte(M25P16_SPI_INSTANCE, command); + + DISABLE_M25P16; +} + +/** + * The flash requires this write enable command to be sent before commands that would cause + * a write like program and erase. + */ +static void m25p16_writeEnable() +{ + m25p16_performOneByteCommand(M25P16_INSTRUCTION_WRITE_ENABLE); + + // Assume that we're about to do some writing, so the device is just about to become busy + couldBeBusy = true; +} + +static uint8_t m25p16_readStatus() +{ + uint8_t command[2] = {M25P16_INSTRUCTION_READ_STATUS_REG, 0}; + uint8_t in[2]; + + ENABLE_M25P16; + + spiTransfer(M25P16_SPI_INSTANCE, in, command, sizeof(command)); + + DISABLE_M25P16; + + return in[1]; +} + +bool m25p16_isReady() +{ + // If couldBeBusy is false, don't bother to poll the flash chip for its status + couldBeBusy = couldBeBusy && ((m25p16_readStatus() & M25P16_STATUS_FLAG_WRITE_IN_PROGRESS) != 0); + + return !couldBeBusy; +} + +bool m25p16_waitForReady(uint32_t timeoutMillis) +{ + uint32_t time = millis(); + while (!m25p16_isReady()) { + if (millis() - time > timeoutMillis) { + return false; + } + } + + return true; +} + +/** + * Read chip identification and geometry information (into global `geometry`). + * + * Returns true if we get valid ident, false if something bad happened like there is no M25P16. + */ +static bool m25p16_readIdentification() +{ + uint8_t out[] = { M25P16_INSTRUCTION_RDID, 0, 0, 0}; + uint8_t in[4]; + + delay(50); // short delay required after initialisation of SPI device instance. + + /* Just in case transfer fails and writes nothing, so we don't try to verify the ID against random garbage + * from the stack: + */ + in[1] = 0; + + ENABLE_M25P16; + + spiTransfer(M25P16_SPI_INSTANCE, in, out, sizeof(out)); + + // Clearing the CS bit terminates the command early so we don't have to read the chip UID: + DISABLE_M25P16; + + // Check manufacturer, memory type, and capacity + if (in[1] == 0x20 && in[2] == 0x20 && in[3] == 0x15) { + // In the future we can support other chip geometries here: + geometry.sectors = 32; + geometry.pagesPerSector = 256; + geometry.pageSize = 256; + + geometry.sectorSize = geometry.pagesPerSector * geometry.pageSize; + geometry.totalSize = geometry.sectorSize * geometry.sectors; + + couldBeBusy = true; // Just for luck we'll assume the chip could be busy even though it isn't specced to be + + return true; + } + + geometry.sectors = 0; + geometry.pagesPerSector = 0; + geometry.pageSize = 0; + + geometry.sectorSize = 0; + geometry.totalSize = 0; + + return false; +} + +/** + * Initialize the driver, must be called before any other routines. + * + * Attempts to detect a connected m25p16. If found, true is returned and device capacity can be fetched with + * m25p16_getGeometry(). + */ +bool m25p16_init() +{ + //Maximum speed for standard READ command is 20mHz, other commands tolerate 25mHz + spiSetDivisor(M25P16_SPI_INSTANCE, SPI_18MHZ_CLOCK_DIVIDER); + + return m25p16_readIdentification(); +} + +/** + * Erase a sector full of bytes to all 1's at the given byte offset in the flash chip. + */ +void m25p16_eraseSector(uint32_t address) +{ + uint8_t out[] = { M25P16_INSTRUCTION_SECTOR_ERASE, (address >> 16) & 0xFF, (address >> 8) & 0xFF, address & 0xFF}; + + m25p16_waitForReady(SECTOR_ERASE_TIMEOUT_MILLIS); + + m25p16_writeEnable(); + + ENABLE_M25P16; + + spiTransfer(M25P16_SPI_INSTANCE, NULL, out, sizeof(out)); + + DISABLE_M25P16; +} + +void m25p16_eraseCompletely() +{ + m25p16_waitForReady(BULK_ERASE_TIMEOUT_MILLIS); + + m25p16_writeEnable(); + + m25p16_performOneByteCommand(M25P16_INSTRUCTION_BULK_ERASE); +} + +void m25p16_pageProgramBegin(uint32_t address) +{ + uint8_t command[] = { M25P16_INSTRUCTION_PAGE_PROGRAM, (address >> 16) & 0xFF, (address >> 8) & 0xFF, address & 0xFF}; + + m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS); + + m25p16_writeEnable(); + + ENABLE_M25P16; + + spiTransfer(M25P16_SPI_INSTANCE, NULL, command, sizeof(command)); +} + +void m25p16_pageProgramContinue(const uint8_t *data, int length) +{ + spiTransfer(M25P16_SPI_INSTANCE, NULL, data, length); +} + +void m25p16_pageProgramFinish() +{ + DISABLE_M25P16; +} + +/** + * Write bytes to a flash page. Address must not cross a page boundary. + * + * Bits can only be set to zero, not from zero back to one again. In order to set bits to 1, use the erase command. + * + * Length must be smaller than the page size. + * + * This will wait for the flash to become ready before writing begins. + * + * Datasheet indicates typical programming time is 0.8ms for 256 bytes, 0.2ms for 64 bytes, 0.05ms for 16 bytes. + * (Although the maximum possible write time is noted as 5ms). + * + * If you want to write multiple buffers (whose sum of sizes is still not more than the page size) then you can + * break this operation up into one beginProgram call, one or more continueProgram calls, and one finishProgram call. + */ +void m25p16_pageProgram(uint32_t address, const uint8_t *data, int length) +{ + m25p16_pageProgramBegin(address); + + m25p16_pageProgramContinue(data, length); + + m25p16_pageProgramFinish(); +} + +/** + * Read `length` bytes into the provided `buffer` from the flash starting from the given `address` (which need not lie + * on a page boundary). + * + * Waits up to DEFAULT_TIMEOUT_MILLIS milliseconds for the flash to become ready before reading. + * + * The number of bytes actually read is returned, which can be zero if an error or timeout occurred. + */ +int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length) +{ + uint8_t command[] = { M25P16_INSTRUCTION_READ_BYTES, (address >> 16) & 0xFF, (address >> 8) & 0xFF, address & 0xFF}; + + if (!m25p16_waitForReady(DEFAULT_TIMEOUT_MILLIS)) { + return 0; + } + + ENABLE_M25P16; + + spiTransfer(M25P16_SPI_INSTANCE, NULL, command, sizeof(command)); + spiTransfer(M25P16_SPI_INSTANCE, buffer, NULL, length); + + DISABLE_M25P16; + + return length; +} + +/** + * Fetch information about the detected flash chip layout. + * + * Can be called before calling m25p16_init() (the result would have totalSize = 0). + */ +const flashGeometry_t* m25p16_getGeometry() +{ + return &geometry; +} diff --git a/src/main/drivers/flash_m25p16.h b/src/main/drivers/flash_m25p16.h new file mode 100644 index 000000000..79d782715 --- /dev/null +++ b/src/main/drivers/flash_m25p16.h @@ -0,0 +1,39 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include +#include "flash.h" + +bool m25p16_init(); + +void m25p16_eraseSector(uint32_t address); +void m25p16_eraseCompletely(); + +void m25p16_pageProgram(uint32_t address, const uint8_t *data, int length); + +void m25p16_pageProgramBegin(uint32_t address); +void m25p16_pageProgramContinue(const uint8_t *data, int length); +void m25p16_pageProgramFinish(); + +int m25p16_readBytes(uint32_t address, uint8_t *buffer, int length); + +bool m25p16_isReady(); +bool m25p16_waitForReady(uint32_t timeoutMillis); + +const flashGeometry_t* m25p16_getGeometry(); diff --git a/src/main/io/flashfs.c b/src/main/io/flashfs.c new file mode 100644 index 000000000..b271cec3e --- /dev/null +++ b/src/main/io/flashfs.c @@ -0,0 +1,563 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +/** + * This provides a stream interface to a flash chip if one is present. + * + * On statup, call flashfsInit() after initialising the flash chip in order to init the filesystem. This will + * result in the file pointer being pointed at the first free block found, or at the end of the device if the + * flash chip is full. + * + * Note that bits can only be set to 0 when writing, not back to 1 from 0. You must erase sectors in order + * to bring bits back to 1 again. + * + * In future, we can add support for multiple different flash chips by adding a flash device driver vtable + * and make calls through that, at the moment flashfs just calls m25p16_* routines explicitly. + */ + +#include +#include +#include + +#include "drivers/flash_m25p16.h" +#include "flashfs.h" + +static uint8_t flashWriteBuffer[FLASHFS_WRITE_BUFFER_SIZE]; + +/* The position of our head and tail in the circular flash write buffer. + * + * The head is the index that a byte would be inserted into on writing, while the tail is the index of the + * oldest byte that has yet to be written to flash. + * + * When the circular buffer is empty, head == tail + */ +static uint8_t bufferHead = 0, bufferTail = 0; + +// The position of the buffer's tail in the overall flash address space: +static uint32_t tailAddress = 0; +// The index of the tail within the flash page it is inside +static uint16_t tailIndexInPage = 0; + +static bool shouldFlush = false; + +static void flashfsClearBuffer() +{ + bufferTail = bufferHead = 0; + shouldFlush = false; +} + +static bool flashfsBufferIsEmpty() +{ + return bufferTail == bufferHead; +} + +static void flashfsSetTailAddress(uint32_t address) +{ + tailAddress = address; + + if (m25p16_getGeometry()->pageSize > 0) { + tailIndexInPage = tailAddress % m25p16_getGeometry()->pageSize; + } +} + +void flashfsEraseCompletely() +{ + m25p16_eraseCompletely(); + + flashfsClearBuffer(); + + flashfsSetTailAddress(0); +} + +/** + * Start and end must lie on sector boundaries, or they will be rounded out to sector boundaries such that + * all the bytes in the range [start...end) are erased. + */ +void flashfsEraseRange(uint32_t start, uint32_t end) +{ + const flashGeometry_t *geometry = m25p16_getGeometry(); + + if (geometry->sectorSize <= 0) + return; + + // Round the start down to a sector boundary + int startSector = start / geometry->sectorSize; + + // And the end upward + int endSector = end / geometry->sectorSize; + int endRemainder = end % geometry->sectorSize; + + if (endRemainder > 0) { + endSector++; + } + + for (int i = startSector; i < endSector; i++) { + m25p16_eraseSector(i * geometry->sectorSize); + } +} + +/** + * Return true if the flash is not currently occupied with an operation. + */ +bool flashfsIsReady() +{ + return m25p16_isReady(); +} + +uint32_t flashfsGetSize() +{ + return m25p16_getGeometry()->totalSize; +} + +const flashGeometry_t* flashfsGetGeometry() +{ + return m25p16_getGeometry(); +} + +static uint32_t flashfsTransmitBufferUsed() +{ + if (bufferHead >= bufferTail) + return bufferHead - bufferTail; + + return FLASHFS_WRITE_BUFFER_SIZE - bufferTail + bufferHead; +} + +/** + * Write the given buffers to flash sequentially at the current tail address, advancing the tail address after + * each write. + * + * In synchronous mode, waits for the flash to become ready before writing so that every byte requested can be written. + * + * In asynchronous mode, if the flash is busy, then the write is aborted and the routine returns immediately. + * In this case the returned number of bytes written will be less than the total amount requested. + * + * Modifies the supplied buffer pointers and sizes to reflect how many bytes remain in each of them. + * + * bufferCount: the number of buffers provided + * buffers: an array of pointers to the beginning of buffers + * bufferSizes: an array of the sizes of those buffers + * sync: true if we should wait for the device to be idle before writes, otherwise if the device is busy the + * write will be aborted and this routine will return immediately. + * + * Returns the number of bytes written + */ +static uint32_t flashfsWriteBuffers(uint8_t const **buffers, uint32_t *bufferSizes, int bufferCount, bool sync) +{ + const flashGeometry_t *geometry = m25p16_getGeometry(); + + uint32_t bytesTotal = 0; + + int i; + + for (i = 0; i < bufferCount; i++) { + bytesTotal += bufferSizes[i]; + } + + if (!sync && !m25p16_isReady()) { + return 0; + } + + uint32_t bytesTotalRemaining = bytesTotal; + + while (bytesTotalRemaining > 0) { + uint32_t bytesTotalThisIteration; + uint32_t bytesRemainThisIteration; + + /* + * Each page needs to be saved in a separate program operation, so + * if we would cross a page boundary, only write up to the boundary in this iteration: + */ + if (tailIndexInPage + bytesTotalRemaining > geometry->pageSize) { + bytesTotalThisIteration = geometry->pageSize - tailIndexInPage; + } else { + bytesTotalThisIteration = bytesTotalRemaining; + } + + // Are we at EOF already? Abort. + if (flashfsIsEOF()) { + // May as well throw away any buffered data + flashfsClearBuffer(); + + break; + } + + m25p16_pageProgramBegin(tailAddress); + + bytesRemainThisIteration = bytesTotalThisIteration; + + for (i = 0; i < bufferCount; i++) { + if (bufferSizes[i] > 0) { + // Is buffer larger than our write limit? Write our limit out of it + if (bufferSizes[i] >= bytesRemainThisIteration) { + m25p16_pageProgramContinue(buffers[i], bytesRemainThisIteration); + + buffers[i] += bytesRemainThisIteration; + bufferSizes[i] -= bytesRemainThisIteration; + + bytesRemainThisIteration = 0; + break; + } else { + // We'll still have more to write after finishing this buffer off + m25p16_pageProgramContinue(buffers[i], bufferSizes[i]); + + bytesRemainThisIteration -= bufferSizes[i]; + + buffers[i] += bufferSizes[i]; + bufferSizes[i] = 0; + } + } + } + + m25p16_pageProgramFinish(); + + bytesTotalRemaining -= bytesTotalThisIteration; + + // Advance the cursor in the file system to match the bytes we wrote + flashfsSetTailAddress(tailAddress + bytesTotalThisIteration); + + /* + * We'll have to wait for that write to complete before we can issue the next one, so if + * the user requested asynchronous writes, break now. + */ + if (!sync) + break; + } + + return bytesTotal - bytesTotalRemaining; +} + +/* + * Since the buffered data might wrap around the end of the circular buffer, we can have two segments of data to write, + * an initial portion and a possible wrapped portion. + * + * This routine will fill the details of those buffers into the provided arrays, which must be at least 2 elements long. + */ +static void flashfsGetDirtyDataBuffers(uint8_t const *buffers[], uint32_t bufferSizes[]) +{ + buffers[0] = flashWriteBuffer + bufferTail; + buffers[1] = flashWriteBuffer + 0; + + if (bufferHead >= bufferTail) { + bufferSizes[0] = bufferHead - bufferTail; + bufferSizes[1] = 0; + } else { + bufferSizes[0] = FLASHFS_WRITE_BUFFER_SIZE - bufferTail; + bufferSizes[1] = bufferHead; + } +} + +/** + * Get the current offset of the file pointer within the volume. + */ +uint32_t flashfsGetOffset() +{ + uint8_t const * buffers[2]; + uint32_t bufferSizes[2]; + + // Dirty data in the buffers contributes to the offset + + flashfsGetDirtyDataBuffers(buffers, bufferSizes); + + return tailAddress + bufferSizes[0] + bufferSizes[1]; +} + +/** + * Called after bytes have been written from the buffer to advance the position of the tail by the given amount. + */ +static void flashfsAdvanceTailInBuffer(uint32_t delta) +{ + bufferTail += delta; + + // Wrap tail around the end of the buffer + if (bufferTail >= FLASHFS_WRITE_BUFFER_SIZE) { + bufferTail -= FLASHFS_WRITE_BUFFER_SIZE; + } + + if (flashfsBufferIsEmpty()) { + flashfsClearBuffer(); // Bring buffer pointers back to the start to be tidier + } +} + +/** + * If the flash is ready to accept writes, flush the buffer to it, otherwise schedule + * a flush for later and return immediately. + * + * Returns true if all data in the buffer has been flushed to the device. + */ +bool flashfsFlushAsync() +{ + if (flashfsBufferIsEmpty()) { + shouldFlush = false; + return true; // Nothing to flush + } + + uint8_t const * buffers[2]; + uint32_t bufferSizes[2]; + uint32_t bytesWritten; + + flashfsGetDirtyDataBuffers(buffers, bufferSizes); + bytesWritten = flashfsWriteBuffers(buffers, bufferSizes, 2, false); + flashfsAdvanceTailInBuffer(bytesWritten); + + shouldFlush = !flashfsBufferIsEmpty(); + + return flashfsBufferIsEmpty(); +} + +/** + * Wait for the flash to become ready and begin flushing any buffered data to flash. + * + * The flash will still be busy some time after this sync completes, but space will + * be freed up to accept more writes in the write buffer. + */ +void flashfsFlushSync() +{ + if (flashfsBufferIsEmpty()) { + shouldFlush = false; + return; // Nothing to flush + } + + uint8_t const * buffers[2]; + uint32_t bufferSizes[2]; + + flashfsGetDirtyDataBuffers(buffers, bufferSizes); + flashfsWriteBuffers(buffers, bufferSizes, 2, true); + + // We've written our entire buffer now: + flashfsClearBuffer(); +} + +void flashfsSeekAbs(uint32_t offset) +{ + flashfsFlushSync(); + + flashfsSetTailAddress(offset); +} + +void flashfsSeekRel(int32_t offset) +{ + flashfsFlushSync(); + + flashfsSetTailAddress(tailAddress + offset); +} + +/** + * Write the given byte asynchronously to the flash. If the buffer overflows, data is silently discarded. + */ +void flashfsWriteByte(uint8_t byte) +{ + flashWriteBuffer[bufferHead++] = byte; + + if (bufferHead >= FLASHFS_WRITE_BUFFER_SIZE) { + bufferHead = 0; + } + + if (shouldFlush || flashfsTransmitBufferUsed() >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) { + flashfsFlushAsync(); + } +} + +/** + * Write the given buffer to the flash either synchronously or asynchronously depending on the 'sync' parameter. + * + * If writing asynchronously, data will be silently discarded if the buffer overflows. + * If writing synchronously, the routine will block waiting for the flash to become ready so will never drop data. + */ +void flashfsWrite(const uint8_t *data, unsigned int len, bool sync) +{ + uint8_t const * buffers[3]; + uint32_t bufferSizes[3]; + + // There could be two dirty buffers to write out already: + flashfsGetDirtyDataBuffers(buffers, bufferSizes); + + // Plus the buffer the user supplied: + buffers[2] = data; + bufferSizes[2] = len; + + /* + * Would writing this data to our buffer cause our buffer to reach the flush threshold? If so try to write through + * to the flash now + */ + if (shouldFlush || bufferSizes[0] + bufferSizes[1] + bufferSizes[2] >= FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN) { + uint32_t bytesWritten; + + // Attempt to write all three buffers through to the flash asynchronously + bytesWritten = flashfsWriteBuffers(buffers, bufferSizes, 3, false); + + if (bufferSizes[0] == 0 && bufferSizes[1] == 0) { + // We wrote all the data that was previously buffered + flashfsClearBuffer(); + + if (bufferSizes[2] == 0) { + // And we wrote all the data the user supplied! Job done! + return; + } + } else { + // We only wrote a portion of the old data, so advance the tail to remove the bytes we did write from the buffer + flashfsAdvanceTailInBuffer(bytesWritten); + } + + // Is the remainder of the data to be written too big to fit in the buffers? + if (bufferSizes[0] + bufferSizes[1] + bufferSizes[2] > FLASHFS_WRITE_BUFFER_USABLE) { + if (sync) { + // Write it through synchronously + flashfsWriteBuffers(buffers, bufferSizes, 3, true); + flashfsClearBuffer(); + } else { + /* + * Silently drop the data the user asked to write (i.e. no-op) since we can't buffer it and they + * requested async. + */ + } + + return; + } + + // Fall through and add the remainder of the incoming data to our buffer + data = buffers[2]; + len = bufferSizes[2]; + } + + // Buffer up the data the user supplied instead of writing it right away + + // First write the portion before we wrap around the end of the circular buffer + unsigned int bufferBytesBeforeWrap = FLASHFS_WRITE_BUFFER_SIZE - bufferHead; + + unsigned int firstPortion = len < bufferBytesBeforeWrap ? len : bufferBytesBeforeWrap; + + memcpy(flashWriteBuffer + bufferHead, data, firstPortion); + + bufferHead += firstPortion; + + data += firstPortion; + len -= firstPortion; + + // If we wrap the head around, write the remainder to the start of the buffer (if any) + if (bufferHead == FLASHFS_WRITE_BUFFER_SIZE) { + memcpy(flashWriteBuffer + 0, data, len); + + bufferHead = len; + } +} + +/** + * Read `len` bytes from the given address into the supplied buffer. + * + * Returns the number of bytes actually read which may be less than that requested. + */ +int flashfsReadAbs(uint32_t address, uint8_t *buffer, unsigned int len) +{ + int bytesRead; + + // Did caller try to read past the end of the volume? + if (address + len > flashfsGetSize()) { + // Truncate their request + len = flashfsGetSize() - address; + } + + // Since the read could overlap data in our dirty buffers, force a sync to clear those first + flashfsFlushSync(); + + bytesRead = m25p16_readBytes(address, buffer, len); + + return bytesRead; +} + +/** + * Find the offset of the start of the free space on the device (or the size of the device if it is full). + */ +int flashfsIdentifyStartOfFreeSpace() +{ + /* Find the start of the free space on the device by examining the beginning of blocks with a binary search, + * looking for ones that appear to be erased. We can achieve this with good accuracy because an erased block + * is all bits set to 1, which pretty much never appears in reasonable size substrings of blackbox logs. + * + * To do better we might write a volume header instead, which would mark how much free space remains. But keeping + * a header up to date while logging would incur more writes to the flash, which would consume precious write + * bandwidth and block more often. + */ + + enum { + /* We can choose whatever power of 2 size we like, which determines how much wastage of free space we'll have + * at the end of the last written data. But smaller blocksizes will require more searching. + */ + FREE_BLOCK_SIZE = 65536, + + /* We don't expect valid data to ever contain this many consecutive uint32_t's of all 1 bits: */ + FREE_BLOCK_TEST_SIZE_INTS = 4, // i.e. 16 bytes + FREE_BLOCK_TEST_SIZE_BYTES = FREE_BLOCK_TEST_SIZE_INTS * sizeof(uint32_t), + }; + + union { + uint8_t bytes[FREE_BLOCK_TEST_SIZE_BYTES]; + uint32_t ints[FREE_BLOCK_TEST_SIZE_INTS]; + } testBuffer; + + int left = 0; + int right = flashfsGetSize() / FREE_BLOCK_SIZE; + int mid, result = right; + int i; + bool blockErased; + + while (left < right) { + mid = (left + right) / 2; + + m25p16_readBytes(mid * FREE_BLOCK_SIZE, testBuffer.bytes, FREE_BLOCK_TEST_SIZE_BYTES); + + // Checking the buffer 4 bytes at a time like this is probably faster than byte-by-byte, but I didn't benchmark it :) + blockErased = true; + for (i = 0; i < FREE_BLOCK_TEST_SIZE_INTS; i++) { + if (testBuffer.ints[i] != 0xFFFFFFFF) { + blockErased = false; + break; + } + } + + if (blockErased) { + /* This erased block might be the leftmost erased block in the volume, but we'll need to continue the + * search leftwards to find out: + */ + result = mid; + + right = mid; + } else { + left = mid + 1; + } + } + + return result * FREE_BLOCK_SIZE; +} + +/** + * Returns true if the file pointer is at the end of the device. + */ +bool flashfsIsEOF() { + return tailAddress >= flashfsGetSize(); +} + +/** + * Call after initializing the flash chip in order to set up the filesystem. + */ +void flashfsInit() +{ + // If we have a flash chip present at all + if (flashfsGetSize() > 0) { + // Start the file pointer off at the beginning of free space so caller can start writing immediately + flashfsSeekAbs(flashfsIdentifyStartOfFreeSpace()); + } +} diff --git a/src/main/io/flashfs.h b/src/main/io/flashfs.h new file mode 100644 index 000000000..4188b6f9d --- /dev/null +++ b/src/main/io/flashfs.h @@ -0,0 +1,52 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#include + +#include "drivers/flash.h" + +#define FLASHFS_WRITE_BUFFER_SIZE 128 +#define FLASHFS_WRITE_BUFFER_USABLE (FLASHFS_WRITE_BUFFER_SIZE - 1) + +// Automatically trigger a flush when this much data is in the buffer +#define FLASHFS_WRITE_BUFFER_AUTO_FLUSH_LEN 64 + +void flashfsEraseCompletely(); +void flashfsEraseRange(uint32_t start, uint32_t end); + +uint32_t flashfsGetSize(); +uint32_t flashfsGetOffset(); +int flashfsIdentifyStartOfFreeSpace(); +const flashGeometry_t* flashfsGetGeometry(); + +void flashfsSeekAbs(uint32_t offset); +void flashfsSeekRel(int32_t offset); + +void flashfsWriteByte(uint8_t byte); +void flashfsWrite(const uint8_t *data, unsigned int len, bool sync); + +int flashfsReadAbs(uint32_t offset, uint8_t *data, unsigned int len); + +bool flashfsFlushAsync(); +void flashfsFlushSync(); + +void flashfsInit(); + +bool flashfsIsReady(); +bool flashfsIsEOF(); diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index ebd881d9c..29c2f5497 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -52,6 +52,7 @@ #include "io/rc_controls.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/flashfs.h" #include "rx/rx.h" #include "rx/spektrum.h" @@ -119,6 +120,13 @@ static void cliColor(char *cmdline); static void cliMixer(char *cmdline); #endif +#ifdef USE_FLASHFS +static void cliFlashInfo(char *cmdline); +static void cliFlashErase(char *cmdline); +static void cliFlashWrite(char *cmdline); +static void cliFlashRead(char *cmdline); +#endif + // signal that we're in cli mode uint8_t cliMode = 0; @@ -183,6 +191,12 @@ const clicmd_t cmdTable[] = { { "dump", "dump configuration", cliDump }, { "exit", "", cliExit }, { "feature", "list or -val or val", cliFeature }, +#ifdef USE_FLASHFS + { "flash_erase", "erase flash chip", cliFlashErase }, + { "flash_info", "get flash chip details", cliFlashInfo }, + { "flash_read", "read text from the given address", cliFlashRead }, + { "flash_write", "write text to the given address", cliFlashWrite }, +#endif { "get", "get variable value", cliGet }, #ifdef GPS { "gpspassthrough", "passthrough gps to serial", cliGpsPassthrough }, @@ -436,6 +450,7 @@ const clivalue_t valueTable[] = { #ifdef BLACKBOX { "blackbox_rate_num", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_num, 1, 32 }, { "blackbox_rate_denom", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_rate_denom, 1, 32 }, + { "blackbox_device", VAR_UINT8 | MASTER_VALUE, &masterConfig.blackbox_device, 0, 1 }, #endif }; @@ -825,6 +840,88 @@ static void cliServo(char *cmdline) #endif } +#ifdef USE_FLASHFS + +static void cliFlashInfo(char *cmdline) +{ + const flashGeometry_t *layout = flashfsGetGeometry(); + + UNUSED(cmdline); + + printf("Flash sectors=%u, sectorSize=%u, pagesPerSector=%u, pageSize=%u, totalSize=%u, usedSize=%u\r\n", + layout->sectors, layout->sectorSize, layout->pagesPerSector, layout->pageSize, layout->totalSize, flashfsGetOffset()); +} + +static void cliFlashErase(char *cmdline) +{ + UNUSED(cmdline); + + printf("Erasing, please wait...\r\n"); + flashfsEraseCompletely(); + + while (!flashfsIsReady()) { + delay(100); + } + + printf("Done.\r\n"); +} + +static void cliFlashWrite(char *cmdline) +{ + uint32_t address = atoi(cmdline); + char *text = strchr(cmdline, ' '); + + if (!text) { + printf("Missing text to write.\r\n"); + } else { + flashfsSeekAbs(address); + flashfsWrite((uint8_t*)text, strlen(text), true); + flashfsFlushSync(); + + printf("Wrote %u bytes at %u.\r\n", strlen(text), address); + } +} + +static void cliFlashRead(char *cmdline) +{ + uint32_t address = atoi(cmdline); + uint32_t length; + int i; + + uint8_t buffer[32]; + + char *nextArg = strchr(cmdline, ' '); + + if (!nextArg) { + printf("Missing length argument.\r\n"); + } else { + length = atoi(nextArg); + + printf("Reading %u bytes at %u:\r\n", length, address); + + while (length > 0) { + int bytesRead; + + bytesRead = flashfsReadAbs(address, buffer, length < sizeof(buffer) ? length : sizeof(buffer)); + + for (i = 0; i < bytesRead; i++) { + cliWrite(buffer[i]); + } + + length -= bytesRead; + address += bytesRead; + + if (bytesRead == 0) { + //Assume we reached the end of the volume or something fatal happened + break; + } + } + printf("\r\n"); + } +} + +#endif + static void dumpValues(uint16_t mask) { uint32_t i; diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 092d587f0..942c009a8 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -50,6 +50,7 @@ #include "io/gimbal.h" #include "io/serial.h" #include "io/ledstrip.h" +#include "io/flashfs.h" #include "telemetry/telemetry.h" @@ -232,6 +233,10 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER; // DEPRECATED - Use MSP_BUILD_INFO instead #define MSP_BF_BUILD_INFO 69 //out message build date as well as some space for future expansion +#define MSP_DATAFLASH_SUMMARY 70 //out message - get description of dataflash chip +#define MSP_DATAFLASH_READ 71 //out message - get content of dataflash chip +#define MSP_DATAFLASH_ERASE 72 //in message - erase dataflash chip + // // Multwii original MSP commands // @@ -434,24 +439,24 @@ static uint32_t read32(void) return t; } -static void headSerialResponse(uint8_t err, uint8_t s) +static void headSerialResponse(uint8_t err, uint8_t responseBodySize) { serialize8('$'); serialize8('M'); serialize8(err ? '!' : '>'); currentPort->checksum = 0; // start calculating a new checksum - serialize8(s); + serialize8(responseBodySize); serialize8(currentPort->cmdMSP); } -static void headSerialReply(uint8_t s) +static void headSerialReply(uint8_t responseBodySize) { - headSerialResponse(0, s); + headSerialResponse(0, responseBodySize); } -static void headSerialError(uint8_t s) +static void headSerialError(uint8_t responseBodySize) { - headSerialResponse(1, s); + headSerialResponse(1, responseBodySize); } static void tailSerialReply(void) @@ -531,6 +536,46 @@ reset: } } +static void serializeDataflashSummaryReply(void) +{ + headSerialReply(1 + 3 * 4); +#ifdef USE_FLASHFS + const flashGeometry_t *geometry = flashfsGetGeometry(); + serialize8(flashfsIsReady() ? 1 : 0); + serialize32(geometry->sectors); + serialize32(geometry->totalSize); + serialize32(flashfsGetOffset()); // Effectively the current number of bytes stored on the volume +#else + serialize8(0); + serialize32(0); + serialize32(0); + serialize32(0); +#endif +} + +#ifdef USE_FLASHFS +static void serializeDataflashReadReply(uint32_t address, uint8_t size) +{ + uint8_t buffer[128]; + int bytesRead; + + if (size > sizeof(buffer)) { + size = sizeof(buffer); + } + + headSerialReply(4 + size); + + serialize32(address); + + // bytesRead will be lower than that requested if we reach end of volume + bytesRead = flashfsReadAbs(address, buffer, size); + + for (int i = 0; i < bytesRead; i++) { + serialize8(buffer[i]); + } +} +#endif + static void resetMspPort(mspPort_t *mspPortToReset, serialPort_t *serialPort, mspPortUsage_e usage) { memset(mspPortToReset, 0, sizeof(mspPort_t)); @@ -1158,6 +1203,21 @@ static bool processOutCommand(uint8_t cmdMSP) } break; #endif + + case MSP_DATAFLASH_SUMMARY: + serializeDataflashSummaryReply(); + break; + +#ifdef USE_FLASHFS + case MSP_DATAFLASH_READ: + { + uint32_t readAddress = read32(); + + serializeDataflashReadReply(readAddress, 128); + } + break; +#endif + case MSP_BF_BUILD_INFO: headSerialReply(11 + 4 + 4); for (i = 0; i < 11; i++) @@ -1371,6 +1431,13 @@ static bool processInCommand(void) writeEEPROM(); readEEPROM(); break; + +#ifdef USE_FLASHFS + case MSP_DATAFLASH_ERASE: + flashfsEraseCompletely(); + break; +#endif + #ifdef GPS case MSP_SET_RAW_GPS: if (read8()) { diff --git a/src/main/main.c b/src/main/main.c index c1b0363d6..8d64a1cc9 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -46,10 +46,12 @@ #include "drivers/bus_i2c.h" #include "drivers/bus_spi.h" #include "drivers/inverter.h" +#include "drivers/flash_m25p16.h" #include "rx/rx.h" #include "io/serial.h" +#include "io/flashfs.h" #include "io/gps.h" #include "io/escservo.h" #include "io/rc_controls.h" @@ -362,6 +364,14 @@ void init(void) telemetryInit(); #endif +#ifdef USE_FLASHFS + #ifdef NAZE + // naze32 rev5 and above have 16mbit of flash available + m25p16_init(); + #endif + flashfsInit(); +#endif + #ifdef BLACKBOX initBlackbox(); #endif diff --git a/src/main/target/NAZE/hardware_revision.c b/src/main/target/NAZE/hardware_revision.c index 3726c79e4..bed18b72f 100644 --- a/src/main/target/NAZE/hardware_revision.c +++ b/src/main/target/NAZE/hardware_revision.c @@ -58,7 +58,7 @@ void detectHardwareRevision(void) #define SPI_DEVICE_MPU (2) #define M25P16_INSTRUCTION_RDID 0x9F -#define FLASH_M25P16 (0x202015) +#define FLASH_M25P16_ID (0x202015) uint8_t detectSpiDevice(void) { @@ -74,7 +74,7 @@ uint8_t detectSpiDevice(void) DISABLE_SPI_CS; flash_id = in[1] << 16 | in[2] << 8 | in[3]; - if (flash_id == FLASH_M25P16) + if (flash_id == FLASH_M25P16_ID) return SPI_DEVICE_FLASH; // try autodetect MPU diff --git a/src/main/target/NAZE/target.h b/src/main/target/NAZE/target.h index 14a8503af..c19d4f258 100644 --- a/src/main/target/NAZE/target.h +++ b/src/main/target/NAZE/target.h @@ -56,10 +56,19 @@ #define NAZE_SPI_CS_GPIO GPIOB #define NAZE_SPI_CS_PIN GPIO_Pin_12 +// We either have this 16mbit flash chip on SPI or the MPU6500 acc/gyro depending on board revision: +#define M25P16_CS_GPIO NAZE_SPI_CS_GPIO +#define M25P16_CS_PIN NAZE_SPI_CS_PIN +#define M25P16_SPI_INSTANCE NAZE_SPI_INSTANCE + #define MPU6500_CS_GPIO NAZE_SPI_CS_GPIO #define MPU6500_CS_PIN NAZE_SPI_CS_PIN #define MPU6500_SPI_INSTANCE NAZE_SPI_INSTANCE +#define USE_FLASHFS + +#define USE_FLASH_M25P16 + #define GYRO #define USE_GYRO_MPU3050 #define USE_GYRO_MPU6050