From 68ab736b16bbc69f1c0de2e30c02042d9424ccd6 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Mon, 16 Nov 2020 13:34:46 -0800 Subject: [PATCH] Refactor mtd to make available to board startup --- .../common/include/px4_platform_common/init.h | 1 + .../px4_platform_common/mtd_manifest.h | 80 +++ .../px4_platform_common/px4_manifest.h | 102 +++ .../include/px4_platform_common/px4_mtd.h | 80 +++ platforms/nuttx/src/px4/common/CMakeLists.txt | 3 + .../nuttx/src/px4/common/px4_24xxxx_mtd.c | 7 +- platforms/nuttx/src/px4/common/px4_init.cpp | 7 + .../nuttx/src/px4/common/px4_manifest.cpp | 89 +++ platforms/nuttx/src/px4/common/px4_mtd.cpp | 408 ++++++++++++ src/systemcmds/mtd/CMakeLists.txt | 1 - src/systemcmds/mtd/mtd.cpp | 584 ++++-------------- 11 files changed, 895 insertions(+), 467 deletions(-) create mode 100644 platforms/common/include/px4_platform_common/mtd_manifest.h create mode 100644 platforms/common/include/px4_platform_common/px4_manifest.h create mode 100644 platforms/common/include/px4_platform_common/px4_mtd.h rename src/systemcmds/mtd/24xxxx_mtd.c => platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c (99%) create mode 100644 platforms/nuttx/src/px4/common/px4_manifest.cpp create mode 100644 platforms/nuttx/src/px4/common/px4_mtd.cpp diff --git a/platforms/common/include/px4_platform_common/init.h b/platforms/common/include/px4_platform_common/init.h index 3ef8ad9989..e8a135fbf0 100644 --- a/platforms/common/include/px4_platform_common/init.h +++ b/platforms/common/include/px4_platform_common/init.h @@ -35,6 +35,7 @@ __BEGIN_DECLS int px4_platform_init(void); int px4_platform_console_init(void); +int px4_platform_configure(void); __END_DECLS diff --git a/platforms/common/include/px4_platform_common/mtd_manifest.h b/platforms/common/include/px4_platform_common/mtd_manifest.h new file mode 100644 index 0000000000..c2b66f9a2b --- /dev/null +++ b/platforms/common/include/px4_platform_common/mtd_manifest.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once +#include + +typedef enum { + MTD_PARAMETERS = 1, + MTD_WAYPOINTS = 2, + MTD_CALDATA = 3, + MTD_MFT = 4, + MTD_ID = 5, + MTD_NET = 6, +} px4_mtd_types_t; + +typedef struct { + const px4_mtd_types_t type; + const char *path; + const uint32_t nblocks; +} px4_mtd_part_t; + +typedef struct { + const px4_mft_device_t *device; + const uint32_t npart; + const px4_mtd_part_t partd[]; +} px4_mtd_entry_t; + +typedef struct { + const uint32_t nconfigs; + const px4_mtd_entry_t *entries[]; +} px4_mtd_manifest_t; + + +__BEGIN_DECLS +/************************************************************************************ + * Name: px4_mtd_config + * + * Description: + * A board will call this function, to set up the mtd partitions + * + * Input Parameters: + * mtd_list - px4_mtd_config list/count + * + * Returned Value: + * non zero if error + * + ************************************************************************************/ + +__EXPORT int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd); + +__END_DECLS diff --git a/platforms/common/include/px4_platform_common/px4_manifest.h b/platforms/common/include/px4_platform_common/px4_manifest.h new file mode 100644 index 0000000000..7b165b82d7 --- /dev/null +++ b/platforms/common/include/px4_platform_common/px4_manifest.h @@ -0,0 +1,102 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once +#include + +typedef enum { + MFT = 0, + MTD = 1, +} px4_manifest_types_e; + + +typedef struct { + + enum px4_bus_type { + I2C = 0, + SPI = 1, + } bus_type; + + uint32_t devid; +} px4_mft_device_t; + +#define PX4_MK_I2C_DEVID(b,a) ((b) << 16 | ((a) & 0xffff)) +#define PX4_I2C_DEVID_BUS(d) (((d) >> 16) & 0xffff) +#define PX4_I2C_DEVID_ADDR(d) ((d) & 0xffff) + +typedef struct { + const px4_manifest_types_e type; + const void *pmft; +} px4_mft_entry_s; + +typedef struct { + const uint32_t nmft; + const px4_mft_entry_s *mfts; +} px4_mft_s; + +#include "px4_platform_common/mtd_manifest.h" + + +__BEGIN_DECLS +/************************************************************************************ + * Name: board_get_manifest + * + * Description: + * A board will provide this function to return the manifest + * + * Input Parameters: + * mft - a pointer to the receive the manifest + * + * Returned Value: + * non zero if error + * + ************************************************************************************/ + +__EXPORT const px4_mft_s *board_get_manifest(void); + +/************************************************************************************ + * Name: px4_mft_configure + * + * Description: + * The Px4 layer will provide this interface to start/configure the + * hardware. + * + * Input Parameters: + * mft - a pointer to the manifest + * + * Returned Value: + * non zero if error + * + ************************************************************************************/ + +__EXPORT int px4_mft_configure(const px4_mft_s *mft); +__END_DECLS diff --git a/platforms/common/include/px4_platform_common/px4_mtd.h b/platforms/common/include/px4_platform_common/px4_mtd.h new file mode 100644 index 0000000000..a8a95e7372 --- /dev/null +++ b/platforms/common/include/px4_platform_common/px4_mtd.h @@ -0,0 +1,80 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ +#pragma once +#include + +__BEGIN_DECLS + +// The data needed to interface with mtd device's + +typedef struct { + struct mtd_dev_s *mtd_dev; + int *partition_block_counts; + const char **partition_names; + struct mtd_dev_s **part_dev; + uint32_t devid; + unsigned n_partitions_current; +} mtd_instance_s; + +/* + mtd operations + */ + +/* + * Get device an pinter to the array of mtd_instance_s of the system + * count - receives the number of instances pointed to by the pointer + * retunred. + * + * returns: - A pointer to the mtd_instance_s of the system + * This can be Null if there are no mtd instances. + * + */ +__EXPORT mtd_instance_s *px4_mtd_get_instances(unsigned int *count); + +/* + Get device complete geometry or a device + */ + + +__EXPORT int px4_mtd_get_geometry(const mtd_instance_s *instance, unsigned long *blocksize, unsigned long *erasesize, + unsigned long *neraseblocks, unsigned *blkpererase, unsigned *nblocks, + unsigned *partsize); +/* + Get size of a parttion on an instance. + */ +__EXPORT ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, const char *partname); + +FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev, + uint8_t address); + +__END_DECLS diff --git a/platforms/nuttx/src/px4/common/CMakeLists.txt b/platforms/nuttx/src/px4/common/CMakeLists.txt index 19089402e3..385bf7dd35 100644 --- a/platforms/nuttx/src/px4/common/CMakeLists.txt +++ b/platforms/nuttx/src/px4/common/CMakeLists.txt @@ -45,6 +45,9 @@ if(NOT PX4_BOARD MATCHES "px4_io") tasks.cpp px4_nuttx_impl.cpp px4_init.cpp + px4_manifest.cpp + px4_mtd.cpp + px4_24xxxx_mtd.c ) target_link_libraries(px4_layer PRIVATE diff --git a/src/systemcmds/mtd/24xxxx_mtd.c b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c similarity index 99% rename from src/systemcmds/mtd/24xxxx_mtd.c rename to platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c index b90a8daa59..178888b096 100644 --- a/src/systemcmds/mtd/24xxxx_mtd.c +++ b/platforms/nuttx/src/px4/common/px4_24xxxx_mtd.c @@ -51,6 +51,7 @@ ************************************************************************************/ #include +#include #include #include @@ -536,8 +537,9 @@ static int at24c_ioctl(FAR struct mtd_dev_s *dev, int cmd, unsigned long arg) * other functions (such as a block or character driver front end). * ************************************************************************************/ +FAR struct mtd_dev_s *px4_at24c_initialize(FAR struct i2c_master_s *dev, + uint8_t address) -FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_master_s *dev) { FAR struct at24c_dev_s *priv; @@ -554,8 +556,7 @@ FAR struct mtd_dev_s *at24c_initialize(FAR struct i2c_master_s *dev) if (priv) { /* Initialize the allocated structure */ - - priv->addr = CONFIG_AT24XX_ADDR; + priv->addr = address; priv->pagesize = AT24XX_PAGESIZE; priv->npages = AT24XX_NPAGES; diff --git a/platforms/nuttx/src/px4/common/px4_init.cpp b/platforms/nuttx/src/px4/common/px4_init.cpp index 967421e20c..c731ad16b6 100644 --- a/platforms/nuttx/src/px4/common/px4_init.cpp +++ b/platforms/nuttx/src/px4/common/px4_init.cpp @@ -33,6 +33,7 @@ #include #include +#include #include #include #include @@ -119,3 +120,9 @@ int px4_platform_init(void) return PX4_OK; } + +int px4_platform_configure(void) +{ + return px4_mft_configure(board_get_manifest()); + +} diff --git a/platforms/nuttx/src/px4/common/px4_manifest.cpp b/platforms/nuttx/src/px4/common/px4_manifest.cpp new file mode 100644 index 0000000000..c7846da4da --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_manifest.cpp @@ -0,0 +1,89 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_manifest.cpp + * + * manifest utilites + * + * @author David Sidrane + */ + +#ifndef MODULE_NAME +#define MODULE_NAME "PX4_MANIFEST" +#endif + +#include +#include +#include + +#include + +__EXPORT const px4_mft_s *board_get_manifest(void) weak_function; + +/* This is the default manifest when no MTD driver is installed */ +static const px4_mft_entry_s mtd_mft = { + .type = MTD, +}; + +static const px4_mft_s default_mft = { + .nmft = 1, + .mfts = &mtd_mft +}; + + +const px4_mft_s *board_get_manifest(void) +{ + return &default_mft; +} + + +__EXPORT int px4_mft_configure(const px4_mft_s *mft) +{ + + if (mft != nullptr) { + for (uint32_t m = 0; m < mft->nmft; m++) { + switch (mft->mfts[m].type) { + case MTD: + px4_mtd_config(static_cast(mft->mfts[m].pmft)); + break; + + case MFT: + default: + break; + } + } + } + + return 0; +} diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp new file mode 100644 index 0000000000..b3f27d8ff0 --- /dev/null +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -0,0 +1,408 @@ +/**************************************************************************** + * + * Copyright (c) 2020 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file px4_mtd.cpp + * + * mtd services. + * + * @author David Sidrane + */ + +#ifndef MODULE_NAME +#define MODULE_NAME "PX4_MTD" +#endif + +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include + +extern "C" { + struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); + struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, + off_t firstblock, off_t nblocks); +} +static int num_instances = 0; +static mtd_instance_s *instances = nullptr; + + +static int ramtron_attach(mtd_instance_s &instance) +{ +#if !defined(CONFIG_MTD_RAMTRON) + PX4_ERR("Misconfiguration CONFIG_MTD_RAMTRON not set"); + return ENXIO; +#else + + /* initialize the right spi */ + struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(instance.devid)); + + if (spi == nullptr) { + PX4_ERR("failed to locate spi bus"); + return -ENXIO; + } + + /* this resets the spi bus, set correct bus speed again */ + SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); + SPI_SETBITS(spi, 8); + SPI_SETMODE(spi, SPIDEV_MODE3); + SPI_SELECT(spi, instance.devid, false); + + /* start the RAMTRON driver, attempt 5 times */ + + for (int i = 0; i < 5; i++) { + instance.mtd_dev = ramtron_initialize(spi); + + if (instance.mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + PX4_WARN("mtd needed %d attempts to attach", i + 1); + } + + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (instance.mtd_dev == nullptr) { + PX4_ERR("failed to initialize mtd driver"); + return -EIO; + } + + int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)10 * 1000 * 1000); + + if (ret != OK) { + // FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried + // that but setting the bus speed does fail all the time. Which was then exiting and the board would + // not run correctly. So changed to PX4_WARN. + PX4_WARN("failed to set bus speed"); + } + + return 0; +#endif +} + + +static int at24xxx_attach(mtd_instance_s &instance) +{ +#if !defined(PX4_I2C_BUS_MTD) + PX4_ERR("Misconfiguration PX4_I2C_BUS_MTD not set"); + return -ENXIO; +#else + + struct i2c_master_s *i2c = px4_i2cbus_initialize(PX4_I2C_DEVID_BUS(instance.devid)); + + if (i2c == nullptr) { + PX4_ERR("failed to locate I2C bus"); + return -ENXIO; + } + + /* start the MTD driver, attempt 5 times */ + for (int i = 0; i < 5; i++) { + instance.mtd_dev = px4_at24c_initialize(i2c, PX4_I2C_DEVID_ADDR(instance.devid)); + + if (instance.mtd_dev) { + /* abort on first valid result */ + if (i > 0) { + PX4_WARN("EEPROM needed %d attempts to attach", i + 1); + } + + break; + } + } + + /* if last attempt is still unsuccessful, abort */ + if (instance.mtd_dev == nullptr) { + PX4_ERR("failed to initialize EEPROM driver"); + return -EIO; + } + + return 0; +#endif +} + + +int px4_mtd_get_geometry(const mtd_instance_s *instance, unsigned long *blocksize, unsigned long *erasesize, + unsigned long *neraseblocks, + unsigned *blkpererase, unsigned *nblocks, unsigned *partsize) +{ + /* Get the geometry of the FLASH device */ + + FAR struct mtd_geometry_s geo; + + int ret = instance->mtd_dev->ioctl(instance->mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + + if (ret < 0) { + PX4_ERR("mtd->ioctl failed: %d", ret); + return ret; + } + + *blocksize = geo.blocksize; + *erasesize = geo.erasesize; + *neraseblocks = geo.neraseblocks; + + /* Determine the size of each partition. Make each partition an even + * multiple of the erase block size (perhaps not using some space at the + * end of the FLASH). + */ + + *blkpererase = geo.erasesize / geo.blocksize; + *nblocks = (geo.neraseblocks / instance->n_partitions_current) * *blkpererase; + *partsize = *nblocks * geo.blocksize; + + return ret; +} + +/* + get partition size in bytes + */ +ssize_t px4_mtd_get_partition_size(const mtd_instance_s *instance, const char *partname) +{ + unsigned long blocksize, erasesize, neraseblocks; + unsigned blkpererase, nblocks, partsize = 0; + + int ret = px4_mtd_get_geometry(instance, &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); + + if (ret != OK) { + PX4_ERR("Failed to get geometry"); + return 0; + } + + unsigned partn = 0; + + for (unsigned n = 0; n < instance->n_partitions_current; n++) { + if (instance->partition_names[n] != nullptr && + partname != nullptr && + strcmp(instance->partition_names[n], partname) == 0) { + partn = n; + break; + } + } + + return instance->partition_block_counts[partn] * blocksize; +} + +mtd_instance_s *px4_mtd_get_instances(unsigned int *count) +{ + *count = num_instances; + return instances; +} + +// Define the default FRAM usage +#if !defined(CONFIG_MTD_RAMTRON) + +static const px4_mtd_manifest_t default_mtd_config = { +}; + +#else + +const px4_mft_device_t spifram = { // FM25V02A on FMUM 32K 512 X 64 + .bus_type = px4_mft_device_t::SPI, + .devid = SPIDEV_FLASH(0) +}; + +const px4_mtd_entry_t fram = { + .device = &spifram, + .npart = 2, + .partd = { + { + .type = MTD_PARAMETERS, + .path = "/fs/mtd_params", + .nblocks = 32 + }, + { + .type = MTD_WAYPOINTS, + .path = "/fs/mtd_waypoints", + .nblocks = 32 + + } + }, +}; + + +static const px4_mtd_manifest_t default_mtd_config = { + .nconfigs = 1, + .entries = { + &fram, + } +}; +#endif + +int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd) +{ + int rv = -EINVAL; + + const px4_mtd_manifest_t *mtd_list = mft_mtd ? mft_mtd : &default_mtd_config; + + if (mtd_list == nullptr) { + PX4_ERR("Invalid mtd configuration!"); + return rv; + } + + if (mtd_list->nconfigs == 0) { + return 0; + } + + rv = -ENOMEM; + int total_blocks = 0; + + instances = new mtd_instance_s[mtd_list->nconfigs]; + + if (instances == nullptr) { +memoryout: + PX4_ERR("failed to allocate memory!"); + return rv; + } + + for (uint32_t i = 0; i < mtd_list->nconfigs; i++) { + num_instances++; + uint32_t nparts = mtd_list->entries[i]->npart; + instances[i].devid = mtd_list->entries[i]->device->devid; + instances[i].mtd_dev = nullptr; + instances[i].n_partitions_current = 0; + + rv = -ENOMEM; + instances[i].part_dev = new FAR struct mtd_dev_s *[nparts]; + + if (instances[i].part_dev == nullptr) { + goto memoryout; + } + + instances[i].partition_block_counts = new int[nparts]; + + if (instances[i].partition_block_counts == nullptr) { + goto memoryout; + } + + instances[i].partition_names = new const char *[nparts]; + + if (instances[i].partition_names == nullptr) { + goto memoryout; + } + + for (uint32_t p = 0; p < nparts; p++) { + instances[i].partition_block_counts[p] = mtd_list->entries[i]->partd[p].nblocks; + instances[i].partition_names[p] = mtd_list->entries[i]->partd[p].path; + } + + if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::I2C) { + rv = at24xxx_attach(instances[i]); + + } else { + rv = ramtron_attach(instances[i]); + } + + if (rv != 0) { + goto errout; + } + + unsigned long blocksize; + unsigned long erasesize; + unsigned long neraseblocks; + unsigned int blkpererase; + unsigned int nblocks; + unsigned int partsize; + + rv = px4_mtd_get_geometry(&instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); + + if (rv != 0) { + goto errout; + } + + /* Now create MTD FLASH partitions */ + + char blockname[32]; + + unsigned offset; + unsigned part; + + for (offset = 0, part = 0; rv == 0 && part < nparts; offset += instances[i].partition_block_counts[part], part++) { + + /* Create the partition */ + + instances[i].part_dev[part] = mtd_partition(instances[i].mtd_dev, offset, instances[i].partition_block_counts[part]); + + if (instances[i].part_dev[part] == nullptr) { + PX4_ERR("mtd_partition failed. offset=%lu nblocks=%lu", + (unsigned long)offset, (unsigned long)nblocks); + rv = -ENOSPC; + goto errout; + } + + /* Initialize to provide an FTL block driver on the MTD FLASH interface */ + + snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", total_blocks); + + rv = ftl_initialize(total_blocks, instances[i].part_dev[part]); + + if (rv < 0) { + PX4_ERR("ftl_initialize %s failed: %d", blockname, rv); + goto errout; + } + + total_blocks++; + + /* Now create a character device on the block device */ + + rv = bchdev_register(blockname, instances[i].partition_names[part], false); + + if (rv < 0) { + PX4_ERR("bchdev_register %s failed: %d", instances[i].partition_names[part], rv); + goto errout; + } + + instances[i].n_partitions_current++; + } + +errout: + + if (rv < 0) { + PX4_ERR("mtd failure: %d bus %d address %d class %d", + rv, + PX4_I2C_DEVID_BUS(instances[i].devid), + PX4_I2C_DEVID_ADDR(instances[i].devid), + mtd_list->entries[i]->partd[instances[i].n_partitions_current].type); + break; + } + } + + return rv; +} diff --git a/src/systemcmds/mtd/CMakeLists.txt b/src/systemcmds/mtd/CMakeLists.txt index a2ecfe4ffb..1f2d23cb47 100644 --- a/src/systemcmds/mtd/CMakeLists.txt +++ b/src/systemcmds/mtd/CMakeLists.txt @@ -36,7 +36,6 @@ px4_add_module( COMPILE_FLAGS SRCS mtd.cpp - 24xxxx_mtd.c DEPENDS ) diff --git a/src/systemcmds/mtd/mtd.cpp b/src/systemcmds/mtd/mtd.cpp index 69c3203295..0d788ccbfa 100644 --- a/src/systemcmds/mtd/mtd.cpp +++ b/src/systemcmds/mtd/mtd.cpp @@ -43,8 +43,11 @@ #include #include #include +#include +#include #include +#include #include #include #include @@ -70,489 +73,94 @@ extern "C" __EXPORT int mtd_main(int argc, char *argv[]); -#ifndef CONFIG_MTD - -/* create a fake command with decent warning to not confuse users */ -int mtd_main(int argc, char *argv[]) -{ - PX4_WARN("MTD not enabled, skipping."); - return 1; -} - -#else - -struct mtd_instance_s; - -# if defined(BOARD_HAS_MTD_PARTITION_OVERRIDE) -# define MTD_PARTITION_TABLE BOARD_HAS_MTD_PARTITION_OVERRIDE -# else -# define MTD_PARTITION_TABLE {"/fs/mtd_params", "/fs/mtd_waypoints"} -# endif - -/* note, these will be equally sized */ -static const char *partition_names_default[] = MTD_PARTITION_TABLE; -#if defined(BOARD_MTD_PARTITION_TABLE_SIZES) -static const float partition_sizes_default[] = BOARD_MTD_PARTITION_TABLE_SIZES; -#else -# define partition_sizes_default nullptr -#endif - - -#ifdef CONFIG_MTD_RAMTRON -static int ramtron_attach(mtd_instance_s &instance); -#else - -#ifndef PX4_I2C_BUS_MTD -#error "Board needs to define PX4_I2C_BUS_MTD for onboard EEPROM bus" -#endif - -#endif - -#ifdef PX4_I2C_BUS_MTD -static int at24xxx_attach(mtd_instance_s &instance); -#endif - -static int mtd_start(mtd_instance_s &instance, const char *partition_names[], unsigned n_partitions); -static int mtd_erase(const char *partition_names[], unsigned n_partitions); -static int mtd_readtest(const mtd_instance_s &instance, const char *partition_names[], unsigned n_partitions); -static int mtd_rwtest(const mtd_instance_s &instance, const char *partition_names[], unsigned n_partitions); -static int mtd_print_info(int instance); -static int mtd_get_geometry(const mtd_instance_s &instance, unsigned long *blocksize, unsigned long *erasesize, - unsigned long *neraseblocks, - unsigned *blkpererase, unsigned *nblocks, unsigned *partsize); - -struct mtd_instance_s { - int (*attach)(mtd_instance_s &instance); - bool attached; - bool started; - struct mtd_dev_s *mtd_dev; - unsigned n_partitions_current; - int *partition_block_counts; - const float *partition_percentages; - const char **partition_names; -}; - -static mtd_instance_s instances[] = { -#ifdef CONFIG_MTD_RAMTRON - {&ramtron_attach, false, false, nullptr, 0, nullptr, partition_sizes_default, partition_names_default}, -#endif -#ifdef PX4_I2C_BUS_MTD - {&at24xxx_attach, false, false, nullptr, 0, nullptr, nullptr, nullptr}, -#endif -}; -static constexpr int num_instances = arraySize(instances); -static const int n_partitions_default = arraySize(partition_names_default); - -static int -mtd_status(void) +static int mtd_status(void) { int ret = 0; bool running = false; + unsigned int num_instances; - for (int i = 0; i < num_instances; ++i) { - if (instances[i].attached) { - ret |= mtd_print_info(i); - running = true; - } - } - - if (!running) { - PX4_INFO("MTD driver not started"); - return 1; - } - - return ret; -} - -static void print_usage(void) -{ - PRINT_MODULE_DESCRIPTION("Utility to mount and test partitions (based on FRAM/EEPROM storage as defined by the board)"); - - PRINT_MODULE_USAGE_NAME("mtd", "command"); - PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status information"); - - PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Mount partitions"); - PRINT_MODULE_USAGE_COMMAND_DESCR("readtest", "Perform read test"); - PRINT_MODULE_USAGE_COMMAND_DESCR("rwtest", "Perform read-write test"); - PRINT_MODULE_USAGE_COMMAND_DESCR("erase", "Erase partition(s)"); - PRINT_MODULE_USAGE_COMMAND_DESCR("has-secondary", "Check if the board has configured a secondary device"); - - PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'start', 'readtest' and 'rwtest' have an optional instance index:"); - PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 1, "storage index (if the board has multiple storages)", true); - - PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'start', 'readtest', 'rwtest' and 'erase' have an optional parameter:"); - PRINT_MODULE_USAGE_ARG(" [ ...]", - "Partition names (eg. /fs/mtd_params), use system default if not provided", true); -} - -int mtd_main(int argc, char *argv[]) -{ - if (argc >= 2) { - int instance = 0; - int partition_index = 2; - - if (argc > 3 && !strcmp(argv[2], "-i")) { - instance = strtol(argv[3], nullptr, 10); - - if (instance < 0 || instance >= num_instances) { - PX4_ERR("invalid instance"); - return -1; - } - - partition_index += 2; - } - - if (!strcmp(argv[1], "start")) { - - /* start mapping according to user request */ - if (argc > partition_index) { - return mtd_start(instances[instance], (const char **)(argv + partition_index), argc - partition_index); - - } else { - return mtd_start(instances[instance], partition_names_default, n_partitions_default); - } - } - - if (!strcmp(argv[1], "readtest")) { - if (argc > partition_index) { - return mtd_readtest(instances[instance], (const char **)(argv + partition_index), argc - partition_index); - - } else { - return mtd_readtest(instances[instance], partition_names_default, n_partitions_default); - } - } - - if (!strcmp(argv[1], "rwtest")) { - if (argc > partition_index) { - return mtd_rwtest(instances[instance], (const char **)(argv + partition_index), argc - partition_index); - - } else { - return mtd_rwtest(instances[instance], partition_names_default, n_partitions_default); - } - } - - if (!strcmp(argv[1], "status")) { - return mtd_status(); - } + const mtd_instance_s *instances = px4_mtd_get_instances(&num_instances); - if (!strcmp(argv[1], "erase")) { - if (argc > partition_index) { - return mtd_erase((const char **)(argv + partition_index), argc - partition_index); + if (instances) { + for (unsigned int i = 0; i < num_instances; ++i) { + if (instances[i].mtd_dev) { - } else { - return mtd_erase(partition_names_default, n_partitions_default); - } - } + unsigned long blocksize; + unsigned long erasesize; + unsigned long neraseblocks; + unsigned int blkpererase; + unsigned int nblocks; + unsigned int partsize; - if (!strcmp(argv[1], "has-secondary")) { - return num_instances > 1 ? 0 : 1; - } - } + ret = px4_mtd_get_geometry(&instances[i], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); - print_usage(); - return 1; -} + if (ret == 0) { -struct mtd_dev_s *ramtron_initialize(FAR struct spi_dev_s *dev); -struct mtd_dev_s *mtd_partition(FAR struct mtd_dev_s *mtd, - off_t firstblock, off_t nblocks); + PX4_INFO("Flash Geometry of instance %i:", i); -#ifdef CONFIG_MTD_RAMTRON -static int -ramtron_attach(mtd_instance_s &instance) -{ - /* initialize the right spi */ - struct spi_dev_s *spi = px4_spibus_initialize(px4_find_spi_bus(SPIDEV_FLASH(0))); + printf(" blocksize: %lu\n", blocksize); + printf(" erasesize: %lu\n", erasesize); + printf(" neraseblocks: %lu\n", neraseblocks); + printf(" No. partitions: %u\n", instances[i].n_partitions_current); - if (spi == nullptr) { - PX4_ERR("failed to locate spi bus"); - return 1; - } - /* this resets the spi bus, set correct bus speed again */ - SPI_SETFREQUENCY(spi, 10 * 1000 * 1000); - SPI_SETBITS(spi, 8); - SPI_SETMODE(spi, SPIDEV_MODE3); - SPI_SELECT(spi, SPIDEV_FLASH(0), false); + unsigned int totalnblocks = 0; + unsigned int totalpartsize = 0; - /* start the RAMTRON driver, attempt 5 times */ + for (unsigned int p = 0; p < instances[i].n_partitions_current; p++) { + FAR struct mtd_geometry_s geo; + ret = instances[i].part_dev[p]->ioctl(instances[i].part_dev[p], MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); + printf(" partition: %u:\n", p); + printf(" name: %s\n", instances[i].partition_names[p]); + printf(" blocks: %u (%u bytes)\n", geo.neraseblocks, erasesize * geo.neraseblocks); + totalnblocks += geo.neraseblocks; + totalpartsize += erasesize * geo.neraseblocks; + } - for (int i = 0; i < 5; i++) { - instance.mtd_dev = ramtron_initialize(spi); + printf(" Device size: %u Blocks (%u bytes)\n", totalnblocks, totalpartsize); + printf(" TOTAL SIZE: %u KiB\n", totalpartsize / 1024); + } - if (instance.mtd_dev) { - /* abort on first valid result */ - if (i > 0) { - PX4_WARN("mtd needed %d attempts to attach", i + 1); + running |= true; } - - break; } } - /* if last attempt is still unsuccessful, abort */ - if (instance.mtd_dev == nullptr) { - PX4_ERR("failed to initialize mtd driver"); - return 1; - } - - int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_SETSPEED, (unsigned long)10 * 1000 * 1000); - - if (ret != OK) { - // FIXME: From the previous warning call, it looked like this should have been fatal error instead. Tried - // that but setting the bus speed does fail all the time. Which was then exiting and the board would - // not run correctly. So changed to PX4_WARN. - PX4_WARN("failed to set bus speed"); - } - - instance.attached = true; - return 0; -} -#endif - -#ifdef PX4_I2C_BUS_MTD - -static int -at24xxx_attach(mtd_instance_s &instance) -{ - /* find the right I2C */ - struct i2c_master_s *i2c = px4_i2cbus_initialize(PX4_I2C_BUS_MTD); - - if (i2c == nullptr) { - PX4_ERR("failed to locate I2C bus"); - return 1; - } - - /* start the MTD driver, attempt 5 times */ - for (int i = 0; i < 5; i++) { - instance.mtd_dev = at24c_initialize(i2c); - - if (instance.mtd_dev) { - /* abort on first valid result */ - if (i > 0) { - PX4_WARN("EEPROM needed %d attempts to attach", i + 1); - } - - break; - } - } - - /* if last attempt is still unsuccessful, abort */ - if (instance.mtd_dev == nullptr) { - PX4_ERR("failed to initialize EEPROM driver"); - return 1; - } - - instance.attached = true; - return 0; -} -#endif - -static int -mtd_start(mtd_instance_s &instance, const char *partition_names[], unsigned n_partitions) -{ - int ret; - - if (instance.started) { - PX4_ERR("mtd already mounted"); - return 1; - } - - if (!instance.attached) { - ret = instance.attach(instance); - - if (ret != 0) { - return ret; - } - } - - if (!instance.mtd_dev) { - PX4_ERR("Failed to create MTD instance"); - return 1; - } - - unsigned long blocksize, erasesize, neraseblocks; - unsigned blkpererase, nblocks, partsize; - - instance.n_partitions_current = n_partitions; - ret = mtd_get_geometry(instance, &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); - - if (ret) { - return ret; - } - - /* Now create MTD FLASH partitions */ - - FAR struct mtd_dev_s *part[n_partitions]; - char blockname[32]; - - unsigned offset; - unsigned i; - - - instance.partition_block_counts = new int[n_partitions]; - - if (instance.partition_block_counts == nullptr) { - PX4_ERR("mtd_partition failed allocation counts"); - return 1; - } - - instance.partition_names = new const char *[n_partitions]; - - if (instance.partition_names == nullptr) { - PX4_ERR("mtd_partition failed allocation for names"); + if (!running) { + PX4_INFO("MTD driver not started"); return 1; } - for (unsigned int n = 0; n < n_partitions; n++) { - float percentage = instance.partition_percentages == nullptr ? - 100.0f / n_partitions : instance.partition_percentages[n]; - volatile int nb = neraseblocks * (percentage / 100.0f); - instance.partition_block_counts[n] = (nb == 0 ? 1 : nb); - instance.partition_names[n] = nullptr; - } - - for (offset = 0, i = 0; i < n_partitions; offset += instance.partition_block_counts[i], i++) { - - /* Create the partition */ - - part[i] = mtd_partition(instance.mtd_dev, offset, instance.partition_block_counts[i]); - - if (!part[i]) { - PX4_ERR("mtd_partition failed. offset=%lu nblocks=%lu", - (unsigned long)offset, (unsigned long)nblocks); - return 1; - } - - instance.partition_names[i] = strdup(partition_names[i]); - - /* Initialize to provide an FTL block driver on the MTD FLASH interface */ - - ret = -1; - - for (int dev_index = 0; ret != 0; ++dev_index) { - snprintf(blockname, sizeof(blockname), "/dev/mtdblock%d", i + dev_index); - - ret = ftl_initialize(i + dev_index, part[i]); - - if (ret == -EEXIST) { - continue; - } - - if (ret < 0 || dev_index >= 9) { - PX4_ERR("ftl_initialize %s failed: %d", blockname, ret); - return 1; - } - } - - /* Now create a character device on the block device */ - - ret = bchdev_register(blockname, partition_names[i], false); - - if (ret < 0) { - PX4_ERR("bchdev_register %s failed: %d", partition_names[i], ret); - return 1; - } - } - - instance.started = true; - return 0; -} - -int mtd_get_geometry(const mtd_instance_s &instance, unsigned long *blocksize, unsigned long *erasesize, - unsigned long *neraseblocks, - unsigned *blkpererase, unsigned *nblocks, unsigned *partsize) -{ - /* Get the geometry of the FLASH device */ - - FAR struct mtd_geometry_s geo; - - int ret = instance.mtd_dev->ioctl(instance.mtd_dev, MTDIOC_GEOMETRY, (unsigned long)((uintptr_t)&geo)); - - if (ret < 0) { - PX4_ERR("mtd->ioctl failed: %d", ret); - return ret; - } - - *blocksize = geo.blocksize; - *erasesize = geo.erasesize; - *neraseblocks = geo.neraseblocks; - - /* Determine the size of each partition. Make each partition an even - * multiple of the erase block size (perhaps not using some space at the - * end of the FLASH). - */ - - *blkpererase = geo.erasesize / geo.blocksize; - *nblocks = (geo.neraseblocks / instance.n_partitions_current) * *blkpererase; - *partsize = *nblocks * geo.blocksize; - return ret; } -/* - get partition size in bytes - */ -static ssize_t mtd_get_partition_size(const mtd_instance_s &instance, const char *partname) -{ - unsigned long blocksize, erasesize, neraseblocks; - unsigned blkpererase, nblocks, partsize = 0; - - int ret = mtd_get_geometry(instance, &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, &partsize); - - if (ret != OK) { - PX4_ERR("Failed to get geometry"); - return 0; - } - - unsigned partn = 0; - - for (unsigned n = 0; n < instance.n_partitions_current; n++) { - if (instance.partition_names[n] != nullptr && - partname != nullptr && - strcmp(instance.partition_names[n], partname) == 0) { - partn = n; - break; - } - } - - return instance.partition_block_counts[partn] * erasesize; -} - -int mtd_print_info(int instance) +static void print_usage(void) { - unsigned long blocksize, erasesize, neraseblocks; - unsigned blkpererase, nblocks, partsize; - - int ret = mtd_get_geometry(instances[instance], &blocksize, &erasesize, &neraseblocks, &blkpererase, &nblocks, - &partsize); - - if (ret) { - return ret; - } - - PX4_INFO("Flash Geometry of instance %i:", instance); + PRINT_MODULE_DESCRIPTION("Utility to mount and test partitions (based on FRAM/EEPROM storage as defined by the board)"); - printf(" blocksize: %lu\n", blocksize); - printf(" erasesize: %lu\n", erasesize); - printf(" neraseblocks: %lu\n", neraseblocks); - printf(" No. partitions: %u\n", instances[instance].n_partitions_current); - printf(" Partition size: %u Blocks (%u bytes)\n", nblocks, partsize); - printf(" TOTAL SIZE: %u KiB\n", neraseblocks * erasesize / 1024); + PRINT_MODULE_USAGE_NAME("mtd", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print status information"); + PRINT_MODULE_USAGE_COMMAND_DESCR("readtest", "Perform read test"); + PRINT_MODULE_USAGE_COMMAND_DESCR("rwtest", "Perform read-write test"); + PRINT_MODULE_USAGE_COMMAND_DESCR("erase", "Erase partition(s)"); + PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'readtest' and 'rwtest' have an optional instance index:"); + PRINT_MODULE_USAGE_PARAM_INT('i', 0, 0, 1, "storage index (if the board has multiple storages)", true); - return 0; + PRINT_MODULE_USAGE_PARAM_COMMENT("The commands 'readtest', 'rwtest' and 'erase' have an optional parameter:"); + PRINT_MODULE_USAGE_ARG(" [ ...]", + "Partition names (eg. /fs/mtd_params), use system default if not provided", true); } -int -mtd_erase(const char *partition_names[], unsigned n_partitions) +int mtd_erase(mtd_instance_s &instance) { uint8_t v[64]; memset(v, 0xFF, sizeof(v)); - for (uint8_t i = 0; i < n_partitions; i++) { + for (uint8_t i = 0; i < instance.n_partitions_current; i++) { + uint32_t count = 0; - printf("Erasing %s\n", partition_names[i]); - int fd = open(partition_names[i], O_WRONLY); + printf("Erasing %s\n", instance.partition_names[i]); + int fd = open(instance.partition_names[i], O_WRONLY); if (fd == -1) { PX4_ERR("Failed to open partition"); @@ -575,25 +183,22 @@ mtd_erase(const char *partition_names[], unsigned n_partitions) responding on the bus. It relies on the driver returning an error on bad reads (the ramtron driver does return an error) */ -int -mtd_readtest(const mtd_instance_s &instance, const char *partition_names[], unsigned n_partitions) +int mtd_readtest(const mtd_instance_s &instance) { - - uint8_t v[128]; - for (uint8_t i = 0; i < n_partitions; i++) { + for (uint8_t i = 0; i < instance.n_partitions_current; i++) { ssize_t count = 0; - ssize_t expected_size = mtd_get_partition_size(instance, partition_names[i]); + ssize_t expected_size = px4_mtd_get_partition_size(&instance, instance.partition_names[i]); if (expected_size == 0) { PX4_ERR("Failed partition size is 0"); return 1; } - printf("reading %s expecting %u bytes\n", partition_names[i], expected_size); - int fd = open(partition_names[i], O_RDONLY); + printf("reading %s expecting %u bytes\n", instance.partition_names[i], expected_size); + int fd = open(instance.partition_names[i], O_RDONLY); if (fd == -1) { PX4_ERR("Failed to open partition"); @@ -622,24 +227,23 @@ mtd_readtest(const mtd_instance_s &instance, const char *partition_names[], unsi blocks and writes the data back, then reads it again, failing if the data isn't the same */ -int -mtd_rwtest(const mtd_instance_s &instance, const char *partition_names[], unsigned n_partitions) +int mtd_rwtest(const mtd_instance_s &instance) { uint8_t v[128], v2[128]; - for (uint8_t i = 0; i < n_partitions; i++) { + for (uint8_t i = 0; i < instance.n_partitions_current; i++) { ssize_t count = 0; off_t offset = 0; - ssize_t expected_size = mtd_get_partition_size(instance, partition_names[i]); + ssize_t expected_size = px4_mtd_get_partition_size(&instance, instance.partition_names[i]); if (expected_size == 0) { PX4_ERR("Failed partition size is 0"); return 1; } - printf("rwtest %s testing %u bytes\n", partition_names[i], expected_size); - int fd = open(partition_names[i], O_RDWR); + printf("rwtest %s testing %u bytes\n", instance.partition_names[i], expected_size); + int fd = open(instance.partition_names[i], O_RDWR); if (fd == -1) { PX4_ERR("Failed to open partition"); @@ -689,4 +293,58 @@ mtd_rwtest(const mtd_instance_s &instance, const char *partition_names[], unsign return 0; } -#endif +int mtd_main(int argc, char *argv[]) +{ + int myoptind = 1; + const char *myoptarg = NULL; + int ch; + int instance = 0; + + while ((ch = px4_getopt(argc, argv, "i:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'i': + instance = atoi(myoptarg); + break; + + default: + print_usage(); + return -1; + break; + } + } + + if (myoptind < argc) { + unsigned int num_instances; + mtd_instance_s *instances = px4_mtd_get_instances(&num_instances); + + if (instances == nullptr) { + PX4_ERR("Driver not running"); + return -1; + } + + if (instance < 0 || (unsigned) instance >= num_instances) { + PX4_ERR("invalid instance"); + return -1; + } + + if (!strcmp(argv[myoptind], "readtest")) { + return mtd_readtest(instances[instance]); + } + + if (!strcmp(argv[myoptind], "rwtest")) { + return mtd_rwtest(instances[instance]); + } + + + if (!strcmp(argv[myoptind], "status")) { + return mtd_status(); + } + + if (!strcmp(argv[myoptind], "erase")) { + return mtd_erase(instances[instance]); + } + } + + print_usage(); + return 1; +}