diff --git a/ioc/kernel-driver/lw35ioc.c b/ioc/kernel-driver/lw35ioc.c index ff2e637..88f7f9d 100644 --- a/ioc/kernel-driver/lw35ioc.c +++ b/ioc/kernel-driver/lw35ioc.c @@ -8,6 +8,9 @@ #include #include #include +#include +#include +#include // #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,11 +63,25 @@ 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); - lw35ioc_set_hw_reset(serdev, 0); + GPIOD_OUT_HIGH); + + /* 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); -