px4_manifest:Add Queries

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

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

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;

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;
+}

platforms/nuttx/src/px4/common/px4_mtd.cpp

@@ -51,6 +51,7 @@
 
 #include <errno.h>
 #include <stdbool.h>
+#include "systemlib/px4_macros.h"
 
 #include <nuttx/drivers/drivers.h>
 #include <nuttx/spi/spi.h>
@@ -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;
+}

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
+	)

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 <lm@inf.ethz.ch>
+ */
+
+#include <px4_platform_common/px4_config.h>
+#include <px4_platform_common/log.h>
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/spi.h>
+#include <px4_platform_common/px4_manifest.h>
+#include <px4_platform_common/getopt.h>
+
+#include <stdio.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdbool.h>
+#include <unistd.h>
+#include <math.h>
+#include <fcntl.h>
+#include <sys/mount.h>
+#include <sys/ioctl.h>
+#include <sys/stat.h>
+
+#include <nuttx/spi/spi.h>
+#include <nuttx/mtd/mtd.h>
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+#include <nuttx/drivers/drivers.h>
+
+#include <arch/board/board.h>
+
+#include "systemlib/px4_macros.h"
+#include <parameters/param.h>
+
+#include <board_config.h>
+
+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;
+}