[PATCH] misc: add /dev/port device for I/O ports

[Ahmad Fatoum <a.fatoum@pengutronix.de>]

For MMIO, we have /dev/mem and the iomem command, but for I/O ports, we
only have the ioport command and no way to access I/O ports from shell.

Add a /dev/port device that interacts directly with I/O ports.

Signed-off-by: Ahmad Fatoum <a.fatoum@pengutronix.de>
---
 drivers/misc/Kconfig  |   4 ++
 drivers/misc/Makefile |   1 +
 drivers/misc/port.c   | 158 ++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 163 insertions(+)
 create mode 100644 drivers/misc/port.c

diff --git a/drivers/misc/Kconfig b/drivers/misc/Kconfig
index 78c9c193d835..40482f8b88a7 100644
--- a/drivers/misc/Kconfig
+++ b/drivers/misc/Kconfig
@@ -24,6 +24,10 @@ config STATE_DRV
 config DEV_MEM
         bool "Generic memory I/O device (/dev/mem)"
 
+config DEV_PORT
+	bool "Generic port I/O device (/dev/port)"
+	depends on X86 || SANDBOX || COMPILE_TEST
+
 config UBOOTVAR
 	bool "U-Boot environment storage"
 	depends on OFTREE
diff --git a/drivers/misc/Makefile b/drivers/misc/Makefile
index 986f7b1b381b..871af85d5920 100644
--- a/drivers/misc/Makefile
+++ b/drivers/misc/Makefile
@@ -7,6 +7,7 @@ obj-$(CONFIG_JTAG)		+= jtag.o
 obj-$(CONFIG_SRAM)		+= sram.o
 obj-$(CONFIG_STATE_DRV)		+= state.o
 obj-$(CONFIG_DEV_MEM)		+= mem.o
+obj-$(CONFIG_DEV_PORT)		+= port.o
 obj-$(CONFIG_UBOOTVAR)		+= ubootvar.o
 obj-$(CONFIG_STARFIVE_PWRSEQ)	+= starfive-pwrseq.o
 obj-$(CONFIG_STORAGE_BY_UUID)	+= storage-by-uuid.o
diff --git a/drivers/misc/port.c b/drivers/misc/port.c
new file mode 100644
index 000000000000..ddc443eb16df
--- /dev/null
+++ b/drivers/misc/port.c
@@ -0,0 +1,158 @@
+// SPDX-License-Identifier: GPL-2.0-only
+/*
+ * Copyright (c) 2011 Sascha Hauer <s.hauer@pengutronix.de>, Pengutronix
+ */
+
+#include <common.h>
+#include <driver.h>
+#include <fcntl.h>
+#include <init.h>
+
+static ssize_t port_read(struct cdev *cdev, void *buf, size_t count, loff_t offset,
+		 unsigned long flags)
+{
+	struct device *dev = cdev->dev;
+	ssize_t size;
+	int rwsize = (flags & (O_RWSIZE_1 | O_RWSIZE_2 | O_RWSIZE_4));
+
+	if (!dev)
+		return -1;
+
+
+	if (!dev || dev->num_resources < 1)
+		return -1;
+
+	if (resource_size(&dev->resource[0]) > 0 || offset != 0)
+		count = min_t(size_t, count,
+			      resource_size(&dev->resource[0]) - offset);
+
+	size = count;
+
+	/* no rwsize specification given. Use maximum */
+	if (!rwsize)
+		rwsize = 4;
+
+	rwsize = rwsize >> O_RWSIZE_SHIFT;
+
+	count = ALIGN_DOWN(count, rwsize);
+
+	while (count) {
+		switch (rwsize) {
+		case 1:
+			*((u8 *)buf) = inb(offset);
+			break;
+		case 2:
+			*((u16 *)buf) = inw(offset);
+			break;
+		case 4:
+			*((u32  *)buf) = inl(offset);
+			break;
+		}
+		buf += rwsize;
+		offset += rwsize;
+		count -= rwsize;
+	}
+
+	return size;
+}
+
+static ssize_t port_write(struct cdev *cdev, const void *buf, size_t count,
+			  loff_t offset, unsigned long flags)
+{
+	struct device *dev = cdev->dev;
+	ssize_t size;
+	int rwsize = (flags & (O_RWSIZE_1 | O_RWSIZE_2 | O_RWSIZE_4));
+
+	if (!dev)
+		return -1;
+
+	if (!dev || dev->num_resources < 1)
+		return -1;
+
+	if (resource_size(&dev->resource[0]) > 0 || offset != 0)
+		count = min_t(size_t, count, resource_size(&dev->resource[0]) - offset);
+	size = count;
+
+	/* no rwsize specification given. Use maximum */
+	if (!rwsize)
+		rwsize = 4;
+
+	rwsize = rwsize >> O_RWSIZE_SHIFT;
+
+	count = ALIGN_DOWN(count, rwsize);
+
+	while (count) {
+		switch (rwsize) {
+		case 1:
+			outb(*((u8 *)buf), offset);
+			break;
+		case 2:
+			outw(*((u16 *)buf), offset);
+			break;
+		case 4:
+			outl(*((u32 *)buf), offset);
+			break;
+		}
+		buf += rwsize;
+		offset += rwsize;
+		count -= rwsize;
+	}
+
+	return size;
+}
+
+static struct cdev_operations portops = {
+	.read  = port_read,
+	.write = port_write,
+};
+
+static int port_probe(struct device *dev)
+{
+	struct cdev *cdev;
+
+	cdev = xzalloc(sizeof (*cdev));
+	dev->priv = cdev;
+
+	cdev->name = (char*)dev->resource[0].name;
+	cdev->size = resource_size(&dev->resource[0]);
+
+	cdev->ops = &portops;
+	cdev->dev = dev;
+
+	devfs_create(cdev);
+
+	return 0;
+}
+
+static struct driver port_drv = {
+	.name  = "port",
+	.probe = port_probe,
+};
+
+static int port_init(void)
+{
+	struct device *dev;
+	struct resource res = {
+		.start = 0,
+		.end = IO_SPACE_LIMIT,
+		.flags = IORESOURCE_IO,
+		.name = "port",
+	};
+	int ret;
+
+	dev = device_alloc("port", DEVICE_ID_DYNAMIC);
+	if (!dev)
+		return -ENOMEM;
+
+	dev->resource = xmemdup(&res, sizeof(res));
+	dev->num_resources = 1;
+
+	ret = platform_device_register(dev);
+	if (ret)
+		return ret;
+
+	pr_notice("I/O port base %p\n", PCI_IOBASE);
+
+	return platform_driver_register(&port_drv);
+}
+device_initcall(port_init);
-- 
2.39.2