feat: flash firmware onto CAN nodes

Phil-Engljaehringer · Phil-Engljaehringer · commit a4a25afb8aef · 2026-04-10T13:19:55.000+02:00
diff --git a/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp b/src/drivers/uavcan/uavcan_drivers/posix/include/uavcan_posix/firmware_version_checker.hpp
@@ -94,7 +94,7 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
 	 * @return                          True - the class will begin sending update requests.
 	 *                                  False - the node will be ignored, no request will be sent.
 	 */
-	virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID,
+	virtual bool shouldRequestFirmwareUpdate(uavcan::NodeID node_id,
 			const uavcan::protocol::GetNodeInfo::Response &node_info,
 			FirmwareFilePath &out_firmware_file_path)
 	{
@@ -117,13 +117,9 @@ class FirmwareVersionChecker : public uavcan::IFirmwareVersionChecker
 		if (n > 0 && n < (int)sizeof(bin_file_name) - 2) {
 			char bin_file_path[MaxBasePathLength + 1];
 
-			// Look on Primary location
-
 			snprintf(bin_file_path, sizeof(bin_file_path), "%s%s",
 				 getFirmwareBasePath().c_str(), bin_file_name);
 
-			// We have a file path, is it a valid image
-
 			AppDescriptor descriptor{0};
 
 			bool found = getFileInfo(bin_file_path, descriptor) == 0;
diff --git a/src/drivers/uavcan/uavcan_main.cpp b/src/drivers/uavcan/uavcan_main.cpp
@@ -619,7 +619,7 @@ UavcanNode::init(uavcan::NodeID node_id, UAVCAN_DRIVER::BusEvent &bus_events)
 	_param_restartnode_client.setCallback(RestartNodeCallback(this, &UavcanNode::cb_restart));
 
 	if (uavcan_enable > 1) {
-		_servers = new UavcanServers(_node, _node_info_retriever);
+		_servers = new UavcanServers(_node, _node_info_retriever, *this);
 
 		if (_servers) {
 			int rv = _servers->init();
@@ -742,6 +742,12 @@ UavcanNode::Run()
 	perf_begin(_cycle_perf);
 	perf_count(_interval_perf);
 
+	if (_servers != nullptr) {
+		if (_servers->checkForNewFirmware()) {
+			_check_fw = true;
+		}
+	}
+
 	for (auto &br : _sensor_bridges) {
 		br->update();
 	}
diff --git a/src/drivers/uavcan/uavcan_servers.cpp b/src/drivers/uavcan/uavcan_servers.cpp
@@ -56,6 +56,10 @@
 #include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
 #include <uavcan_posix/firmware_version_checker.hpp>
 
+static constexpr size_t FW_DB_LINE_SIZE = 256; // key(~15) + '=' + filename(NAME_MAX) + '\n'
+static const char FW_DB_PATH[] = UAVCAN_FIRMWARE_PATH "/FW.db";
+static const char FW_DB_TMP_PATH[] = UAVCAN_FIRMWARE_PATH "/FW.db.tmp";
+
 /**
  * @file uavcan_servers.cpp
  *
@@ -65,12 +69,14 @@
  *         David Sidrane <david_s5@nscdg.com>
  */
 
-UavcanServers::UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever) :
+UavcanServers::UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever,
+			     UavcanNode &uavcan_node) :
 	_server_instance(node, _storage_backend, _tracer),
 	_fileserver_backend(node),
 	_fw_upgrade_trigger(node, _fw_version_checker),
 	_fw_server(node, _fileserver_backend),
-	_node_info_retriever(node_info_retriever)
+	_node_info_retriever(node_info_retriever),
+	_uavcan_node(uavcan_node)
 {
 }
 
@@ -135,6 +141,7 @@ int UavcanServers::init()
 	Check for firmware in the root directory, move it to appropriate location on
 	the SD card, as defined by the APDesc.
 	*/
+	validateFwDatabase();
 	migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH);
 
 	/*  Start the Node   */
@@ -159,6 +166,7 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 	int rv;
 	char srcpath[maxlen + 1];
 	char dstpath[maxlen + 1];
+	uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor{};
 
 	if (stat(sd_path, &sb) != 0 || !S_ISDIR(sb.st_mode)) {
 		rv = mkdir(sd_path, S_IRWXU | S_IRWXG | S_IRWXO);
@@ -169,11 +177,6 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 		}
 	}
 
-	// Phase 1: collect .bin filenames with directory open, then close
-	static constexpr int MaxBinFiles = 10;
-	char bin_names[MaxBinFiles][NAME_MAX + 1];
-	int bin_count = 0;
-
 	DIR *const sd_root_dir = opendir(sd_root_path);
 
 	if (!sd_root_dir) {
@@ -182,7 +185,7 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 
 	struct dirent *dev_dirent = NULL;
 
-	while ((dev_dirent = readdir(sd_root_dir)) != nullptr && bin_count < MaxBinFiles) {
+	while ((dev_dirent = readdir(sd_root_dir)) != nullptr) {
 		if (DIRENT_ISFILE(dev_dirent->d_type) && strstr(dev_dirent->d_name, ".bin") != nullptr) {
 			size_t filename_len = strlen(dev_dirent->d_name);
 			size_t srcpath_len = sd_root_path_len + 1 + filename_len;
@@ -192,34 +195,27 @@ void UavcanServers::migrateFWFromRoot(const char *sd_path, const char *sd_root_p
 				continue;
 			}
 
-			strncpy(bin_names[bin_count], dev_dirent->d_name, NAME_MAX);
-			bin_names[bin_count][NAME_MAX] = '\0';
-			bin_count++;
-		}
-	}
-
-	(void)closedir(sd_root_dir);
-
-	// Phase 2: process collected files with directory closed
-	for (int i = 0; i < bin_count; i++) {
-		uavcan_posix::FirmwareVersionChecker::AppDescriptor descriptor{0};
+			snprintf(srcpath, sizeof(srcpath), "%s%s", sd_root_path, dev_dirent->d_name);
 
-		snprintf(srcpath, sizeof(srcpath), "%s%s", sd_root_path, bin_names[i]);
-
-		if (uavcan_posix::FirmwareVersionChecker::getFileInfo(srcpath, descriptor, 1024) != 0) {
-			continue;
-		}
+			if (uavcan_posix::FirmwareVersionChecker::getFileInfo(srcpath, descriptor, 1024) != 0) {
+				continue;
+			}
 
-		if (descriptor.image_crc == 0) {
-			continue;
-		}
+			if (descriptor.image_crc == 0) {
+				continue;
+			}
 
-		snprintf(dstpath, sizeof(dstpath), "%s/%d.bin", sd_path, descriptor.board_id);
+			snprintf(dstpath, sizeof(dstpath), "%s/%d.bin", sd_path, descriptor.board_id);
 
-		if (copyFw(dstpath, srcpath) >= 0) {
-			unlink(srcpath);
+			if (copyFw(dstpath, srcpath) >= 0) {
+				updateFwDatabase(dstpath, dev_dirent->d_name);
+				unlink(srcpath);
+				_new_firmware = true;
+			}
 		}
 	}
+
+	(void)closedir(sd_root_dir);
 }
 
 int UavcanServers::copyFw(const char *dst, const char *src)
@@ -277,3 +273,183 @@ int UavcanServers::copyFw(const char *dst, const char *src)
 
 	return rv;
 }
+
+void UavcanServers::updateFwDatabase(const char *dst, const char *src_filename)
+{
+	const char *key = dst + strlen(UAVCAN_FIRMWARE_PATH);
+
+	if (*key == '/') {
+		key++;
+	}
+
+	char new_line[FW_DB_LINE_SIZE];
+	snprintf(new_line, sizeof(new_line), "%s=%s\n", key, src_filename);
+	const size_t new_line_len = strlen(new_line);
+
+	char key_prefix[64];
+	snprintf(key_prefix, sizeof(key_prefix), "%s=", key);
+	const size_t key_prefix_len = strlen(key_prefix);
+
+	int src_fd = open(FW_DB_PATH, O_RDONLY);
+	int dst_fd = open(FW_DB_TMP_PATH, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+
+	if (dst_fd < 0) {
+		if (src_fd >= 0) { close(src_fd); }
+
+		PX4_WARN("updateFwDatabase: couldn't open tmp file");
+		return;
+	}
+
+	bool found = false;
+	char line[FW_DB_LINE_SIZE];
+	int pos = 0;
+	char c;
+
+	if (src_fd >= 0) {
+		while (read(src_fd, &c, 1) == 1) {
+			if (c == '\n' || pos >= (int)sizeof(line) - 2) {
+				if (c == '\n') { line[pos++] = c; }
+
+				line[pos] = '\0';
+
+				if (strncmp(line, key_prefix, key_prefix_len) == 0) {
+					write(dst_fd, new_line, new_line_len);
+					found = true;
+
+				} else {
+					write(dst_fd, line, pos);
+				}
+
+				pos = 0;
+
+			} else {
+				line[pos++] = c;
+			}
+		}
+
+		// flush any trailing line without newline
+		if (pos > 0) {
+			line[pos] = '\0';
+
+			if (strncmp(line, key_prefix, key_prefix_len) == 0) {
+				write(dst_fd, new_line, new_line_len);
+				found = true;
+
+			} else {
+				write(dst_fd, line, pos);
+			}
+		}
+
+		close(src_fd);
+	}
+
+	if (!found) {
+		write(dst_fd, new_line, new_line_len);
+	}
+
+	close(dst_fd);
+	rename(FW_DB_TMP_PATH, FW_DB_PATH);
+}
+
+void UavcanServers::validateFwDatabase()
+{
+	int src_fd = open(FW_DB_PATH, O_RDONLY);
+
+	if (src_fd < 0) {
+		return; // no DB yet, nothing to validate
+	}
+
+	int dst_fd = open(FW_DB_TMP_PATH, O_WRONLY | O_CREAT | O_TRUNC, 0666);
+
+	if (dst_fd < 0) {
+		close(src_fd);
+		PX4_WARN("validateFwDatabase: couldn't open tmp file");
+		return;
+	}
+
+	bool changed = false;
+	char line[FW_DB_LINE_SIZE];
+	int pos = 0;
+	char c;
+
+	auto process_line = [&](int len) {
+		line[len] = '\0';
+		const char *eq = strchr(line, '=');
+
+		if (eq != nullptr) {
+			char key[64];
+			const size_t key_len = (size_t)(eq - line);
+
+			if (key_len < sizeof(key)) {
+				memcpy(key, line, key_len);
+				key[key_len] = '\0';
+
+				char full_path[UAVCAN_MAX_PATH_LENGTH + 1];
+				snprintf(full_path, sizeof(full_path), "%s/%s", UAVCAN_FIRMWARE_PATH, key);
+
+				struct stat sb;
+
+				if (stat(full_path, &sb) == 0 && S_ISREG(sb.st_mode)) {
+					write(dst_fd, line, len);
+
+				} else {
+					PX4_INFO("validateFwDatabase: removing stale entry for '%s'", key);
+					changed = true;
+				}
+			}
+
+		} else {
+			changed = true; // malformed — drop it
+		}
+	};
+
+	while (read(src_fd, &c, 1) == 1) {
+		if (c == '\n' || pos >= (int)sizeof(line) - 2) {
+			if (c == '\n') { line[pos++] = c; }
+
+			process_line(pos);
+			pos = 0;
+
+		} else {
+			line[pos++] = c;
+		}
+	}
+
+	if (pos > 0) {
+		process_line(pos);
+	}
+
+	close(src_fd);
+	close(dst_fd);
+
+	if (changed) {
+		rename(FW_DB_TMP_PATH, FW_DB_PATH);
+
+	} else {
+		unlink(FW_DB_TMP_PATH);
+	}
+}
+
+void UavcanServers::onFirmwareCurrent(uavcan::NodeID node_id, uint16_t board_id)
+{
+	const uint8_t nid = node_id.get();
+
+	if (nid >= sizeof(_configured_nodes) || _configured_nodes[nid]) {
+		return;
+	}
+
+	_configured_nodes[nid] = true;
+}
+
+bool UavcanServers::checkForNewFirmware()
+{
+	if (hrt_elapsed_time(&_last_fw_migration_check) < 30_s) {
+		return false;
+	}
+
+	_last_fw_migration_check = hrt_absolute_time();
+
+	_new_firmware = false;
+	migrateFWFromRoot(UAVCAN_FIRMWARE_PATH, UAVCAN_SD_ROOT_PATH);
+	return _new_firmware;
+}
diff --git a/src/drivers/uavcan/uavcan_servers.hpp b/src/drivers/uavcan/uavcan_servers.hpp
@@ -34,6 +34,7 @@
 #pragma once
 
 #include <px4_platform_common/px4_config.h>
+#include <drivers/drv_hrt.h>
 
 #include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
 #include <uavcan/protocol/file_server.hpp>
@@ -47,6 +48,8 @@
 
 #include "uavcan_module.hpp"
 
+class UavcanNode;
+
 /**
  * @file uavcan_servers.hpp
  *
@@ -62,18 +65,27 @@
 class UavcanServers
 {
 public:
-	UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever);
+	UavcanServers(uavcan::INode &node, uavcan::NodeInfoRetriever &node_info_retriever, UavcanNode &uavcan_node);
 	~UavcanServers() = default;
 
 	int init();
+	bool checkForNewFirmware();
 
 	bool guessIfAllDynamicNodesAreAllocated() { return _server_instance.guessIfAllDynamicNodesAreAllocated(); }
 
 private:
 
+	void onFirmwareCurrent(uavcan::NodeID node_id, uint16_t board_id);
+
 	void unpackFwFromROMFS(const char *sd_path, const char *romfs_path);
 	void migrateFWFromRoot(const char *sd_path, const char *sd_root_path);
 	int copyFw(const char *dst, const char *src);
+	void updateFwDatabase(const char *dst, const char *src_filename);
+	void validateFwDatabase();
+
+	bool _new_firmware{false};
+
+	hrt_abstime _last_fw_migration_check{0};
 
 	uavcan_posix::dynamic_node_id_server::FileEventTracer _tracer;
 	uavcan_posix::dynamic_node_id_server::FileStorageBackend _storage_backend;
@@ -84,4 +96,8 @@ class UavcanServers
 	uavcan::BasicFileServer _fw_server;
 
 	uavcan::NodeInfoRetriever &_node_info_retriever;
+	UavcanNode &_uavcan_node;
+
+	// Tracks nodes whose config has already been applied this session
+	bool _configured_nodes[128] {};
 };
