nvme: Add support for set-reg command to write nvme register

Note: For fabrics properties not supported to set by the command.

Signed-off-by: Tokunori Ikegami <ikegami.t@gmail.com>

Author: ikegami-t

common.h

@@ -32,7 +32,24 @@ static inline uint64_t mmio_read64(void *addr)
 	low = le32_to_cpu(*p);
 	high = le32_to_cpu(*(p + 1));
 
-	return ((uint64_t) high << 32) | low;
+	return ((uint64_t)high << 32) | low;
 }
 
+static inline void mmio_write32(void *addr, uint32_t value)
+{
+	leint32_t *p = addr;
+
+	*p = cpu_to_le32(value);
+}
+
+/* Access 64-bit registers as 2 32-bit; Some devices fail 64-bit MMIO. */
+static inline void mmio_write64(void *addr, uint64_t value)
+{
+	uint32_t *p = addr;
+	uint32_t low = value;
+	uint32_t high = value >> 32;
+
+	*p = cpu_to_le32(low);
+	*(p + 1) = cpu_to_le32(high);
+}
 #endif

nvme-builtin.h

@@ -89,6 +89,7 @@ COMMAND_LIST(
 	ENTRY("subsystem-reset", "Resets the subsystem", subsystem_reset)
 	ENTRY("ns-rescan", "Rescans the NVME namespaces", ns_rescan)
 	ENTRY("show-regs", "Shows the controller registers or properties. Requires character device", show_registers)
+	ENTRY("set-reg", "Set a register and show the resulting value", set_register)
 	ENTRY("get-reg", "Get a register and show the resulting value", get_register)
 	ENTRY("discover", "Discover NVMeoF subsystems", discover_cmd)
 	ENTRY("connect-all", "Discover and Connect to NVMeoF subsystems", connect_all_cmd)

nvme.c

@@ -5214,14 +5214,15 @@ static int nvme_get_properties(int fd, void **pbar)
 	return err;
 }
 
-static void *mmap_registers(nvme_root_t r, struct nvme_dev *dev)
+static void *mmap_registers_prot(nvme_root_t r, struct nvme_dev *dev, int prot)
 {
 	nvme_ctrl_t c = NULL;
 	nvme_ns_t n = NULL;
 
 	char path[512];
 	void *membase;
 	int fd;
+	int flags = prot | PROT_WRITE ? O_RDWR : O_RDONLY;
 
 	c = nvme_scan_ctrl(r, dev->name);
 	if (c) {
@@ -5239,15 +5240,15 @@ static void *mmap_registers(nvme_root_t r, struct nvme_dev *dev)
 		nvme_free_ns(n);
 	}
 
-	fd = open(path, O_RDONLY);
+	fd = open(path, flags);
 	if (fd < 0) {
 		if (map_log_level(0, false) >= LOG_DEBUG)
 			nvme_show_error("%s did not find a pci resource, open failed %s",
 					dev->name, strerror(errno));
 		return NULL;
 	}
 
-	membase = mmap(NULL, getpagesize(), PROT_READ, MAP_SHARED, fd, 0);
+	membase = mmap(NULL, getpagesize(), prot, MAP_SHARED, fd, 0);
 	if (membase == MAP_FAILED) {
 		if (map_log_level(0, false) >= LOG_DEBUG) {
 			fprintf(stderr, "%s failed to map. ", dev->name);
@@ -5260,6 +5261,11 @@ static void *mmap_registers(nvme_root_t r, struct nvme_dev *dev)
 	return membase;
 }
 
+static void *mmap_registers(nvme_root_t r, struct nvme_dev *dev)
+{
+	return mmap_registers_prot(r, dev, PROT_READ);
+}
+
 static int show_registers(int argc, char **argv, struct command *cmd, struct plugin *plugin)
 {
 	const char *desc = "Reads and shows the defined NVMe controller registers\n"
@@ -5393,6 +5399,7 @@ static int get_register(int argc, char **argv, struct command *cmd, struct plugi
 	r = nvme_scan(NULL);
 	bar = mmap_registers(r, dev);
 	if (!bar) {
+		err = -errno;
 		err = nvme_get_properties(dev_fd(dev), &bar);
 		if (err)
 			goto free_tree;
@@ -5409,6 +5416,72 @@ static int get_register(int argc, char **argv, struct command *cmd, struct plugi
 	return err;
 }
 
+static void nvme_set_register(void *bar, int offset, uint64_t value)
+{
+	if (nvme_is_64bit_reg(offset))
+		mmio_write64(bar + offset, value);
+	else
+		mmio_write32(bar + offset, value);
+}
+
+static int set_register(int argc, char **argv, struct command *cmd, struct plugin *plugin)
+{
+	const char *desc =
+	    "Writes and shows the defined NVMe controller register";
+	const char *offset = "the offset of the register";
+	const char *value = "the value of the register to be set";
+
+	_cleanup_nvme_dev_ struct nvme_dev *dev = NULL;
+	int err;
+	nvme_root_t r;
+	void *bar;
+
+	struct config {
+		int offset;
+		uint64_t value;
+	};
+
+	struct config cfg = {
+		.offset	= -1,
+		.value	= 0,
+	};
+
+	NVME_ARGS(opts, cfg,
+		  OPT_UINT("offset",  'O', &cfg.offset, offset),
+		  OPT_SUFFIX("value", 'V', &cfg.value,  value));
+
+	err = parse_and_open(&dev, argc, argv, desc, opts);
+	if (err)
+		return err;
+
+	if (!argconfig_parse_seen(opts, "offset")) {
+		nvme_show_error("offset required param");
+		return -EINVAL;
+	}
+
+	if (!argconfig_parse_seen(opts, "value")) {
+		nvme_show_error("value required param");
+		return -EINVAL;
+	}
+
+	r = nvme_scan(NULL);
+	bar = mmap_registers_prot(r, dev, PROT_READ | PROT_WRITE);
+	if (!bar) {
+		nvme_show_error("Failed to map");
+		return -EINVAL;
+	}
+
+	nvme_set_register(bar, cfg.offset, cfg.value);
+
+	printf("set-register: %02x (%s), value: %"PRIx64"\n", cfg.offset,
+	       nvme_register_to_string(cfg.offset), cfg.value);
+
+	munmap(bar, getpagesize());
+	nvme_free_tree(r);
+
+	return err;
+}
+
 static int get_property(int argc, char **argv, struct command *cmd, struct plugin *plugin)
 {
 	const char *desc = "Reads and shows the defined NVMe controller property\n"