From e83a00c6040230705e21dcaa7b79ea68e12d90ba Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Fri, 20 Nov 2020 11:25:51 -0800 Subject: [PATCH] px4_manifest:Add Queries --- .../px4_platform_common/mtd_manifest.h | 25 +++ .../px4_platform_common/px4_manifest.h | 22 +++ .../include/px4_platform_common/px4_mtd.h | 1 + .../nuttx/src/px4/common/px4_manifest.cpp | 24 +++ platforms/nuttx/src/px4/common/px4_mtd.cpp | 51 ++++++ src/systemcmds/mft/CMakeLists.txt | 40 +++++ src/systemcmds/mft/mft.cpp | 161 ++++++++++++++++++ 7 files changed, 324 insertions(+) create mode 100644 src/systemcmds/mft/CMakeLists.txt create mode 100644 src/systemcmds/mft/mft.cpp diff --git a/platforms/common/include/px4_platform_common/mtd_manifest.h b/platforms/common/include/px4_platform_common/mtd_manifest.h index c2b66f9a2b..2ab9514573 100644 --- a/platforms/common/include/px4_platform_common/mtd_manifest.h +++ b/platforms/common/include/px4_platform_common/mtd_manifest.h @@ -41,6 +41,8 @@ typedef enum { MTD_ID = 5, MTD_NET = 6, } px4_mtd_types_t; +#define PX4_MFT_MTD_TYPES {MTD_PARAMETERS, MTD_WAYPOINTS, MTD_CALDATA, MTD_MFT, MTD_ID, MTD_NET} +#define PX4_MFT_MTD_STR_TYPES {"MTD_PARAMETERS", "MTD_WAYPOINTS", "MTD_CALDATA", "MTD_MFT", "MTD_ID", "MTD_NET"} typedef struct { const px4_mtd_types_t type; @@ -77,4 +79,27 @@ __BEGIN_DECLS __EXPORT int px4_mtd_config(const px4_mtd_manifest_t *mft_mtd); +/************************************************************************************ + * Name: px4_mtd_query + * + * Description: + * A Query interface that will lookup a type and either a) verify it exists by + * value. + * + * or it will return the path for a type. + * + * + * Input Parameters: + * type - a string-ized version of px4_mtd_types_t + * value - string to verity is that type. + * get - a pointer to a string to optionally return the path for the type. + * + * Returned Value: + * non zero if error + * 0 (get == null) item by type and value was found. + * 0 (get !=null) item by type's value is returned at get; + * + ************************************************************************************/ +__EXPORT int px4_mtd_query(const char *type, const char *val, const char **get = nullptr); + __END_DECLS diff --git a/platforms/common/include/px4_platform_common/px4_manifest.h b/platforms/common/include/px4_platform_common/px4_manifest.h index 7b165b82d7..82184ecb53 100644 --- a/platforms/common/include/px4_platform_common/px4_manifest.h +++ b/platforms/common/include/px4_platform_common/px4_manifest.h @@ -36,8 +36,12 @@ typedef enum { MFT = 0, MTD = 1, + LAST_MFT_TYPE } px4_manifest_types_e; +/* must match px4_manifest_types_e */ +#define PX4_MFT_TYPES {MFT, MTD} +#define PX4_MFT_STR_TYPES {"MFT", "MTD"} typedef struct { @@ -99,4 +103,22 @@ __EXPORT const px4_mft_s *board_get_manifest(void); ************************************************************************************/ __EXPORT int px4_mft_configure(const px4_mft_s *mft); + +/************************************************************************************ + * 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_query(const px4_mft_s *mft, px4_manifest_types_e type, + const char *sub, const char *val); __END_DECLS diff --git a/platforms/common/include/px4_platform_common/px4_mtd.h b/platforms/common/include/px4_platform_common/px4_mtd.h index a8a95e7372..11f5edb21f 100644 --- a/platforms/common/include/px4_platform_common/px4_mtd.h +++ b/platforms/common/include/px4_platform_common/px4_mtd.h @@ -40,6 +40,7 @@ __BEGIN_DECLS typedef struct { struct mtd_dev_s *mtd_dev; int *partition_block_counts; + int *partition_types; const char **partition_names; struct mtd_dev_s **part_dev; uint32_t devid; diff --git a/platforms/nuttx/src/px4/common/px4_manifest.cpp b/platforms/nuttx/src/px4/common/px4_manifest.cpp index c7846da4da..6d7e322542 100644 --- a/platforms/nuttx/src/px4/common/px4_manifest.cpp +++ b/platforms/nuttx/src/px4/common/px4_manifest.cpp @@ -87,3 +87,27 @@ __EXPORT int px4_mft_configure(const px4_mft_s *mft) return 0; } + +__EXPORT int px4_mft_query(const px4_mft_s *mft, px4_manifest_types_e type, + const char *sub, const char *val) +{ + int rv = -EINVAL; + + if (mft != nullptr) { + for (uint32_t m = 0; m < mft->nmft; m++) { + if (mft->mfts[m].type == type) + switch (type) { + case MTD: + return px4_mtd_query(sub, val); + break; + + case MFT: + default: + rv = -ENODATA; + break; + } + } + } + + return rv; +} diff --git a/platforms/nuttx/src/px4/common/px4_mtd.cpp b/platforms/nuttx/src/px4/common/px4_mtd.cpp index b3f27d8ff0..7635398d5b 100644 --- a/platforms/nuttx/src/px4/common/px4_mtd.cpp +++ b/platforms/nuttx/src/px4/common/px4_mtd.cpp @@ -51,6 +51,7 @@ #include #include +#include "systemlib/px4_macros.h" #include #include @@ -312,6 +313,12 @@ memoryout: goto memoryout; } + instances[i].partition_types = new int[nparts]; + + if (instances[i].partition_types == nullptr) { + goto memoryout; + } + instances[i].partition_names = new const char *[nparts]; if (instances[i].partition_names == nullptr) { @@ -321,6 +328,7 @@ 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; + instances[i].partition_types[p] = mtd_list->entries[i]->partd[p].type; } if (mtd_list->entries[i]->device->bus_type == px4_mft_device_t::I2C) { @@ -406,3 +414,46 @@ errout: return rv; } + +__EXPORT int px4_mtd_query(const char *sub, const char *val, const char **get) +{ + int rv = -ENODEV; + + if (instances != nullptr) { + + static const char *keys[] = PX4_MFT_MTD_STR_TYPES; + static const px4_mtd_types_t types[] = PX4_MFT_MTD_TYPES; + int key = 0; + + for (unsigned int k = 0; k < arraySize(keys); k++) { + if (!strcmp(keys[k], sub)) { + key = types[k]; + break; + } + } + + + rv = -EINVAL; + + if (key != 0) { + rv = -ENOENT; + + for (int i = 0; i < num_instances; i++) { + for (unsigned n = 0; n < instances[i].n_partitions_current; n++) { + if (instances[i].partition_types[n] == key) { + if (get != nullptr && val == nullptr) { + *get = instances[i].partition_names[n]; + return 0; + } + + if (val != nullptr && strcmp(instances[i].partition_names[n], val) == 0) { + return 0; + } + } + } + } + } + } + + return rv; +} diff --git a/src/systemcmds/mft/CMakeLists.txt b/src/systemcmds/mft/CMakeLists.txt new file mode 100644 index 0000000000..52f12275f8 --- /dev/null +++ b/src/systemcmds/mft/CMakeLists.txt @@ -0,0 +1,40 @@ +############################################################################ +# +# 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. +# +############################################################################ +px4_add_module( + MODULE systemcmds__mft + MAIN mft + COMPILE_FLAGS + SRCS + mft.cpp + DEPENDS + ) diff --git a/src/systemcmds/mft/mft.cpp b/src/systemcmds/mft/mft.cpp new file mode 100644 index 0000000000..f113e8fa5e --- /dev/null +++ b/src/systemcmds/mft/mft.cpp @@ -0,0 +1,161 @@ +/**************************************************************************** + * + * Copyright (c) 2013-2014 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 mtd.c + * + * mtd service and utility app. + * + * @author Lorenz Meier + */ + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include + +#include "systemlib/px4_macros.h" +#include + +#include + +extern "C" __EXPORT int mft_main(int argc, char *argv[]); + +static int mft_status(void) +{ + return 0; +} + +static void print_usage(void) +{ + PRINT_MODULE_DESCRIPTION("Utility interact with the manifest"); + + PRINT_MODULE_USAGE_NAME("mfd", "command"); + PRINT_MODULE_USAGE_COMMAND_DESCR("query", "Returns true if not existed"); +} + + + +int mft_main(int argc, char *argv[]) +{ + static const char *keys[] = PX4_MFT_STR_TYPES; + static const px4_manifest_types_e types[] = PX4_MFT_TYPES; + + int myoptind = 1; + const char *myoptarg = nullptr; + int ch; + + int silent = 0; + px4_manifest_types_e key = MFT; + const char *value = nullptr; + const char *subject = nullptr; + + while ((ch = px4_getopt(argc, argv, "qk:v:s:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'q': + silent = 1; + break; + + case 'k': + for (unsigned int k = 0; k < arraySize(keys); k++) { + if (!strcmp(keys[k], myoptarg)) { + key = types[k]; + break; + } + } + + break; + + case 'v': + value = myoptarg; + break; + + case 's': + subject = myoptarg; + break; + + default: + print_usage(); + return -1; + break; + } + } + + if (myoptind < argc) { + int rv = 0; + + if (!strcmp(argv[myoptind], "status")) { + return mft_status(); + } + + if (!strcmp(argv[myoptind], "query")) { + if (value && subject) { + rv = px4_mft_query(board_get_manifest(), key, subject, value); + + if (!silent) { + printf("%s\n", value); + } + + return rv; + } + + return -EINVAL; + } + + } + + print_usage(); + return 1; +}