ioc: kernel-driver: Add IOC charecter device

ioc/kernel-driver/lw35ioc.c

@@ -8,6 +8,9 @@
 #include <linux/types.h>
 #include <linux/input.h>
 #include <linux/gpio/consumer.h>
+#include <linux/uaccess.h>
+#include <linux/sched.h>
+#include <linux/poll.h>
 
 // #define ENABLE_DEBUG ENABLE_DEBUG
 
@@ -20,12 +23,20 @@ static int debug_level = 0;
 module_param(debug_level, int, 0);
 MODULE_PARM_DESC(debug_level, "Debug level");
 
+/* Only for chrdev... man they really suck! */
+static struct serdev_device *static_serdev = NULL;
+static DECLARE_WAIT_QUEUE_HEAD(wq);
+
 static struct of_device_id lw35ioc_dt_ids[] = {
 { .compatible = "brother,lw35", },
 { /* sentinel */ }
 };
 MODULE_DEVICE_TABLE(of, lw35ioc_dt_ids);
 
+/* Chardev */
+#define DEVICE_NAME "lw35"
+#define CLASS_NAME "lw35ioc"
+
 /* Only used for initialization, not for actualy keycode mapping */
 static int keycode_map[] = {
 	KEY_A, KEY_B, KEY_C, KEY_D, KEY_E, KEY_F, KEY_G, KEY_H, KEY_I, KEY_J,
@@ -44,7 +55,7 @@ static int keycode_map[] = {
 	KEY_SYSRQ
 };
 
-enum lw35ioc_state {PASSIVE, PASSTHROUGH, ACTIVE};
+enum lw35ioc_state {PASSIVE = 0, PROG_KBD, PROG_PRI, BYPASS, ACTIVE, STATE_COUNT};
 
 enum lw35ioc_packet_state {SYNC, TYPE, KEYBOARD, PRINTER};
 
@@ -52,10 +63,24 @@ struct lw35ioc_packet {
 	enum lw35ioc_packet_state state;
 };
 
+struct lw35ioc_chardev {
+	int major_number;
+	struct class* class;
+	struct device* device;
+	int open_count;
+};
+
+#define UART_BUF_SIZE 256
+
 struct lw35ioc_priv {
 	struct serdev_device *serdev;
 	struct input_dev *input_dev;
 	struct gpio_desc *gpio_rst; /* GPIO to reset both AVRs */
+	struct lw35ioc_chardev chardev;
+
+	char uart_buf_rx[UART_BUF_SIZE];
+	char *uart_buf_rx_rd;
+	char *uart_buf_rx_wr;
 
 	enum lw35ioc_state state;
 	struct lw35ioc_packet packet;
@@ -118,15 +143,33 @@ static int lw35ioc_receive_buf(struct serdev_device *serdev,
 			       size_t count)
 {
 	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+	int i;
+
 	if (priv->state == ACTIVE)
 		return lw35ioc_process_buf(serdev, data, count);
 
+	for (i = 0; i < count; ++i) {
+#ifdef ENABLE_DEBUG
+		dev_info(&serdev->dev, "  RX [PSV]: %02x\n", data[i]);
+#endif
+		*(priv->uart_buf_rx_wr) = data[i];
+		if (priv->uart_buf_rx_wr == priv->uart_buf_rx + UART_BUF_SIZE - 1)
+			priv->uart_buf_rx_wr = priv->uart_buf_rx;
+		else
+			priv->uart_buf_rx_wr++;
+
+		if (priv->uart_buf_rx_rd == priv->uart_buf_rx_wr) {
+			dev_err(&serdev->dev, "UART RX buffer overflow.\n");
+		}
+	}
+
+	wake_up_interruptible(&wq);
+
 	return count; /* Pretend we read all data */
 }
 
 static void lw35ioc_write_wakeup(struct serdev_device *serdev)
 {
-	dev_info(&serdev->dev, "Write wakeup\n");
 	// schedule work here
 }
 
@@ -140,19 +183,224 @@ static void lw35ioc_set_hw_reset(struct serdev_device *serdev,
 {
 	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
 
-	gpiod_set_value(priv->gpio_rst, !reset_active);
+	gpiod_set_value(priv->gpio_rst, !!reset_active);
+}
+
+static void lw35ioc_reset_ioc_hw(struct serdev_device *serdev)
+{
+	dev_info(&serdev->dev, "Resetting IOC...\n");
+	lw35ioc_set_hw_reset(serdev, 1);
+	usleep_range(90000, 100000);
+	lw35ioc_set_hw_reset(serdev, 0);
+	usleep_range(90000, 100000);
+}
+
+/* Chardev */
+static int lw35ioc_chardev_open(struct inode *inodep, struct file *filep) {
+	struct serdev_device *serdev = static_serdev;
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+
+	filep->private_data = serdev;
+
+	if (!priv->chardev.open_count) {
+		dev_info(&serdev->dev, "Opened chardev.\n");
+
+		if (priv->state != ACTIVE &&
+				priv->state != PASSIVE) {
+			lw35ioc_reset_ioc_hw(serdev);
+
+			switch (priv->state) {
+			case PROG_KBD:
+				serdev_device_write(serdev, "X", 1, 0);
+				dev_info(&serdev->dev, "Put ioc into programming mode: keyboard.\n");
+				break;
+			case PROG_PRI:
+				serdev_device_write(serdev, "Y", 1, 0);
+				dev_info(&serdev->dev, "Put ioc into programming mode: printer.\n");
+				break;
+			case BYPASS:
+				serdev_device_write(serdev, "XX", 2, 0);
+				dev_info(&serdev->dev, "Put ioc into bypass mode.\n");
+				break;
+			default:
+				/* For other modes, we just wait for the reset to be over */
+				usleep_range(1000000, 1010000);
+				break;
+			}
+		}
+	} else {
+		dev_err(&serdev->dev, "Rejected. Device may only be opened once.\n");
+		//return -EBUSY;
+	}
+
+	priv->chardev.open_count++;
+
+	return 0;
+}
+
+static ssize_t lw35ioc_chardev_read(struct file *filep, char *buffer, size_t len, loff_t *offset) {
+	struct serdev_device *serdev = filep->private_data;
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+	int error_count;
+	int available;
+
+	available = priv->uart_buf_rx_wr - priv->uart_buf_rx_rd;
+	if (available < 0)
+		available = 1;
+	if (!available) { /* Wait for data to become available */
+		wait_event_interruptible(wq, priv->uart_buf_rx_rd != priv->uart_buf_rx_wr);
+		available = priv->uart_buf_rx_wr - priv->uart_buf_rx_rd;
+		if (available < 0)
+			available = 1;
+		if (!available)
+			return -EAGAIN;
+	}
+
+	if (available < len)
+		len = available;
+
+	error_count = copy_to_user(buffer, priv->uart_buf_rx_rd, len);
+	len -= error_count;
+	priv->uart_buf_rx_rd += len;
+	if (priv->uart_buf_rx_rd >= priv->uart_buf_rx + UART_BUF_SIZE) {
+		priv->uart_buf_rx_rd -= UART_BUF_SIZE;
+	}
+
+	if (error_count == 0) {
+		return len;
+	} else {
+		dev_err(&serdev->dev, "Failed to read %d characters to chardev.\n",
+			error_count);
+		return -EFAULT;
+	}
+}
+
+char msg[256]; // TODO
+static ssize_t lw35ioc_chardev_write(struct file *filep, const char *buffer, size_t len, loff_t *offset) {
+	struct serdev_device *serdev = filep->private_data;
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+
+	if (len > 256) {
+		len = 256;
+	}
+	copy_from_user(msg, buffer, len);
+	serdev_device_write_buf(serdev, msg, len);
+
+	return len;
+}
+
+static int lw35ioc_chardev_release(struct inode *inodep, struct file *filep){
+	struct serdev_device *serdev = filep->private_data;
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+
+	priv->chardev.open_count--;
+
+	dev_info(&serdev->dev, "Device successfully closed\n");
+	return 0;
+}
+
+#define IOCTL_SET_MODE 42
+
+static long lw35ioc_chardev_ioctl(struct file *filep, unsigned int cmd,
+				  unsigned long arg)
+{
+	struct serdev_device *serdev = filep->private_data;
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+
+	switch (cmd) {
+	case IOCTL_SET_MODE:
+		if (arg >= STATE_COUNT || arg < 0) {
+			dev_err(&serdev->dev, "Invalid driver mode: %lu", arg);
+			return -1;
+		}
+		priv->state = arg;
+		dev_info(&serdev->dev, "Switched driver mode to: %lu. Will be applied with next open().\n", arg);
+		break;
+	default:
+		dev_info(&serdev->dev, "Ignoring IOCTL %u(%lu)\n", cmd, arg);
+		break;
+	}
+
+	return 0;
+}
+
+__poll_t poll(struct file *filep, struct poll_table_struct *ptab)
+{
+	struct serdev_device *serdev = filep->private_data;
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+
+	poll_wait(filep, &wq, ptab);
+
+	if (priv->uart_buf_rx_wr != priv->uart_buf_rx_rd)
+		return EPOLLIN | EPOLLRDNORM;
+
+	return 0;
+}
+
+
+
+static struct file_operations fops =
+{
+	.open = lw35ioc_chardev_open,
+	.read = lw35ioc_chardev_read,
+	.write = lw35ioc_chardev_write,
+	.release = lw35ioc_chardev_release,
+	.unlocked_ioctl = lw35ioc_chardev_ioctl,
+	.poll = poll,
+};
+
+
+static int lw35ioc_cdev_init(struct serdev_device *serdev)
+{
+	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
+
+	priv->chardev.open_count = 0;
+
+	// Try to dynamically allocate a major number for the device
+	priv->chardev.major_number = register_chrdev(0, DEVICE_NAME, &fops);
+	if (priv->chardev.major_number < 0) {
+		dev_err(&serdev->dev, "Failed to register a major number\n");
+		return priv->chardev.major_number;
+	}
+	dev_info(&serdev->dev, "Registered correctly with major number %d\n", priv->chardev.major_number);
+
+	// Register the device class
+	priv->chardev.class = class_create(THIS_MODULE, CLASS_NAME);
+	if (IS_ERR(priv->chardev.class)) {
+		unregister_chrdev(priv->chardev.major_number, DEVICE_NAME);
+		dev_err(&serdev->dev, "Failed to register device class\n");
+		return PTR_ERR(priv->chardev.class);
+	}
+	dev_info(&serdev->dev, "Device class registered correctly\n");
+
+	// Register the device driver
+	priv->chardev.device = device_create(priv->chardev.class, NULL, MKDEV(priv->chardev.major_number, 0), NULL, DEVICE_NAME);
+	if (IS_ERR(priv->chardev.device)) {
+		class_destroy(priv->chardev.class);
+		unregister_chrdev(priv->chardev.major_number, DEVICE_NAME);
+		dev_err(&serdev->dev, "Failed to create the device\n");
+		return PTR_ERR(priv->chardev.device);
+	}
+
+	dev_err(&serdev->dev, "Device class created correctly\n");
+	return 0;
 }
 
 static int lw35ioc_probe(struct serdev_device *serdev)
 {
 	struct lw35ioc_priv *priv = NULL;
-	int ret;
+	int ret = 0;
 	int i;
 	u32 speed = 19200;
 	struct input_dev *input_dev;
 
 	printk(KERN_INFO "lw35ioc: probe\n");
 
+	if (static_serdev) {
+		printk(KERN_ERR "lw35ioc: Can only be loaded once.\n");
+		goto fail_multicheck;
+	}
+
 	/* Allocate memory */
 	priv = devm_kzalloc(&serdev->dev,
 			    sizeof(struct lw35ioc_priv),
@@ -161,8 +409,13 @@ static int lw35ioc_probe(struct serdev_device *serdev)
 		printk(KERN_ERR "lw35ioc: Not enough memory.\n");
 		return -ENOMEM;
 	}
+
 	priv->state = PASSIVE;
+	priv->uart_buf_rx_rd = priv->uart_buf_rx;
+	priv->uart_buf_rx_wr = priv->uart_buf_rx;
+
 	priv->serdev = serdev;
+	static_serdev = serdev;
 	priv->packet.state = SYNC;
 	serdev_device_set_drvdata(serdev, priv);
 	serdev_device_set_client_ops(serdev, &lw35ioc_serdev_ops);
@@ -200,14 +453,23 @@ static int lw35ioc_probe(struct serdev_device *serdev)
 	priv->gpio_rst = gpiod_get_index(&serdev->dev,
 					 "rst",
 					 0,
-					 GPIOD_OUT_LOW);
+					 GPIOD_OUT_HIGH);
-	lw35ioc_set_hw_reset(serdev, 0);
+
+	/* Register character device */
+	ret = lw35ioc_cdev_init(serdev);
+	if (ret) {
+		dev_err(&serdev->dev, "Unable to initialize character device.\n");
+		goto fail_gpio;
+	}
 
 	if (IS_ERR(priv->gpio_rst)) {
 		dev_err(&serdev->dev, "Unable to get reset GPIO.\n");
-		ret = -ENODEV;
+		ret = -ENODEV; /* TODO */
 		goto fail_gpio;
 	}
+
+	lw35ioc_reset_ioc_hw(serdev);
+
 	dev_info(&serdev->dev, "Loaded driver.\n");
 
 	return 0;
@@ -216,6 +478,7 @@ fail_serdev:
 	input_unregister_device(input_dev);
 fail_input:
 	devm_kfree(&serdev->dev, input_dev);
+fail_multicheck:
 	return ret;
 }
 
@@ -223,6 +486,11 @@ static void lw35ioc_remove(struct serdev_device *serdev)
 {
 	struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev);
 
+	device_destroy(priv->chardev.class, MKDEV(priv->chardev.major_number, 0));
+	class_unregister(priv->chardev.class);
+	class_destroy(priv->chardev.class);
+	unregister_chrdev(priv->chardev.major_number, DEVICE_NAME);
+
 	lw35ioc_set_hw_reset(serdev, 1);
 	gpiod_put(priv->gpio_rst);
 	serdev_device_close(priv->serdev);
@@ -243,4 +511,3 @@ static struct serdev_device_driver lw35ioc_driver = {
 };
 
 module_serdev_device_driver(lw35ioc_driver);
-