diff --git a/ioc/kernel-driver/lw35ioc.c b/ioc/kernel-driver/lw35ioc.c index 88f7f9d..71dc256 100644 --- a/ioc/kernel-driver/lw35ioc.c +++ b/ioc/kernel-driver/lw35ioc.c @@ -11,6 +11,7 @@ #include #include #include +#include // #define ENABLE_DEBUG ENABLE_DEBUG @@ -27,6 +28,11 @@ MODULE_PARM_DESC(debug_level, "Debug level"); static struct serdev_device *static_serdev = NULL; static DECLARE_WAIT_QUEUE_HEAD(wq); +void pbuf_work_handler(struct work_struct *work); +DECLARE_WORK(pbuf_work, pbuf_work_handler); + +static DEFINE_SPINLOCK(printer_status_lock); + static struct of_device_id lw35ioc_dt_ids[] = { { .compatible = "brother,lw35", }, { /* sentinel */ } @@ -37,7 +43,7 @@ MODULE_DEVICE_TABLE(of, lw35ioc_dt_ids); #define DEVICE_NAME "lw35" #define CLASS_NAME "lw35ioc" -/* Only used for initialization, not for actualy keycode mapping */ +/* Only used for initialization, not for actual 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, KEY_K, KEY_L, KEY_M, KEY_N, KEY_O, KEY_P, KEY_Q, KEY_R, KEY_S, KEY_T, @@ -61,6 +67,7 @@ enum lw35ioc_packet_state {SYNC, TYPE, KEYBOARD, PRINTER}; struct lw35ioc_packet { enum lw35ioc_packet_state state; + int payload_offset; }; struct lw35ioc_chardev { @@ -70,7 +77,15 @@ struct lw35ioc_chardev { int open_count; }; +struct lw35ioc_printer_status{ + int buffer_free; + int status; + int reserved0; + int reserved1; +}; + #define UART_BUF_SIZE 256 +#define PBUF_SIZE 256 struct lw35ioc_priv { struct serdev_device *serdev; @@ -78,10 +93,17 @@ struct lw35ioc_priv { struct gpio_desc *gpio_rst; /* GPIO to reset both AVRs */ struct lw35ioc_chardev chardev; + struct lw35ioc_printer_status printer_status_temp; + struct lw35ioc_printer_status printer_status; + char uart_buf_rx[UART_BUF_SIZE]; char *uart_buf_rx_rd; char *uart_buf_rx_wr; + char pbuf[PBUF_SIZE]; + unsigned long pbuf_tail; + unsigned long pbuf_head; + enum lw35ioc_state state; struct lw35ioc_packet packet; }; @@ -95,6 +117,7 @@ static int lw35ioc_process_buf(struct serdev_device *serdev, unsigned char c; int ev; int i; + unsigned long lock_flags; #ifdef ENABLE_DEBUG dev_info(&serdev->dev, "IRQ.\n"); @@ -109,6 +132,7 @@ static int lw35ioc_process_buf(struct serdev_device *serdev, if (c == 0xFF) priv->packet.state = TYPE; break; + case TYPE: if (c == 0x55) priv->packet.state = KEYBOARD; @@ -116,7 +140,9 @@ static int lw35ioc_process_buf(struct serdev_device *serdev, priv->packet.state = PRINTER; else priv->packet.state = SYNC; + priv->packet.payload_offset = 0; break; + case KEYBOARD: if (c > 127) { c -= 128; @@ -128,9 +154,46 @@ static int lw35ioc_process_buf(struct serdev_device *serdev, input_sync(priv->input_dev); priv->packet.state = SYNC; break; + case PRINTER: - dev_info(&serdev->dev, "Printer not yet implemented.\n"); - priv->packet.state = SYNC; + switch (priv->packet.payload_offset) { + case 0: /* Status */ + priv->printer_status_temp.status = (int)c; + break; + case 1: /* Buffer status */ + priv->printer_status_temp.buffer_free = (int)c; + break; + case 2: /* Reserved */ + priv->printer_status_temp.reserved0 = (int)c; + break; + case 3: /* Reserved */ + priv->printer_status_temp.reserved1 = (int)c; + + dev_info(&serdev->dev, + "New printer status. status=0x%02X buffer_free=0x%02X [0x%02X,0x%02X].\n", + priv->printer_status_temp.status, + priv->printer_status_temp.buffer_free, + priv->printer_status_temp.reserved0, + priv->printer_status_temp.reserved1); + if (priv->printer_status_temp.status == 0xFF) { + dev_err(&serdev->dev, + "HARDFAULT in printer firmware! Reset required.\n"); + } + spin_lock_irqsave(&printer_status_lock, lock_flags); + memcpy(&priv->printer_status, + &priv->printer_status_temp, + sizeof(priv->printer_status)); + spin_unlock_irqrestore(&printer_status_lock, lock_flags); + + if (priv->printer_status_temp.buffer_free > 0) + schedule_work(&pbuf_work); + + priv->packet.state = SYNC; + break; + default: + break; + } + priv->packet.payload_offset++; break; } } @@ -145,25 +208,31 @@ static int lw35ioc_receive_buf(struct serdev_device *serdev, struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev); int i; - if (priv->state == ACTIVE) + switch (priv->state) { + case ACTIVE: return lw35ioc_process_buf(serdev, data, count); - - for (i = 0; i < count; ++i) { + break; + case PASSIVE: + break; + default: + /* Pass data directly to chardev */ + for (i = 0; i < count; ++i) { #ifdef ENABLE_DEBUG - dev_info(&serdev->dev, " RX [PSV]: %02x\n", data[i]); + 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++; + *(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"); + if (priv->uart_buf_rx_rd == priv->uart_buf_rx_wr) { + dev_err(&serdev->dev, "UART RX buffer overflow.\n"); + } } - } - wake_up_interruptible(&wq); + wake_up_interruptible(&wq); + } return count; /* Pretend we read all data */ } @@ -205,32 +274,38 @@ static int lw35ioc_chardev_open(struct inode *inodep, struct file *filep) { if (!priv->chardev.open_count) { dev_info(&serdev->dev, "Opened chardev.\n"); - if (priv->state != ACTIVE && - priv->state != PASSIVE) { + switch (priv->state) { + case PROG_KBD: 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; - } + serdev_device_write(serdev, "X", 1, 0); + dev_info(&serdev->dev, "Put ioc into programming mode: keyboard.\n"); + break; + case PROG_PRI: + lw35ioc_reset_ioc_hw(serdev); + serdev_device_write(serdev, "Y", 1, 0); + dev_info(&serdev->dev, "Put ioc into programming mode: printer.\n"); + break; + case BYPASS: + lw35ioc_reset_ioc_hw(serdev); + serdev_device_write(serdev, "XX", 2, 0); + dev_info(&serdev->dev, "Put ioc into bypass mode.\n"); + break; + case PASSIVE: + lw35ioc_reset_ioc_hw(serdev); + usleep_range(1000000, 1010000); + dev_info(&serdev->dev, "Put ioc into active mode.\n"); + priv->state = ACTIVE; + break; + case ACTIVE: + /* Don't do anything when active */ + break; + default: + break; } + } else { dev_err(&serdev->dev, "Rejected. Device may only be opened once.\n"); - //return -EBUSY; + //return -EBUSY; // TODO } priv->chardev.open_count++; @@ -251,7 +326,7 @@ static ssize_t lw35ioc_chardev_read(struct file *filep, char *buffer, size_t len 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; + available = 1; /* TODO: We should calculate the remaining block until wraparound here... */ if (!available) return -EAGAIN; } @@ -279,12 +354,61 @@ 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); + unsigned long tail; + int i; + int ret; - if (len > 256) { - len = 256; + switch (priv->state) { + case ACTIVE: + /* Put data into pbuf */ + // TODO: spinlock + if (CIRC_SPACE(priv->pbuf_head, READ_ONCE(priv->pbuf_tail), PBUF_SIZE) < 1) { + dev_err(&serdev->dev, "chdev write: Waiting for space...\n"); + if (ret = (wait_event_interruptible(wq, CIRC_SPACE(priv->pbuf_head, priv->pbuf_tail, PBUF_SIZE) >= 1))) { // TODO: Accessing pbuf_tail without any protection... this might be bad? + dev_err(&serdev->dev, + "chdev write: device closed while waiting. ret: %d\n", + ret); + // TODO: This is not proper handling of this I guess... + return len; + } + dev_err(&serdev->dev, "chdev write: done waiting...\n"); + } + + tail = READ_ONCE(priv->pbuf_tail); + for (i = 0; i < len; ++i) { // TODO: Block copy would be faster... + if (CIRC_SPACE(priv->pbuf_head, tail, PBUF_SIZE) >= 1) { + ret = copy_from_user(priv->pbuf + priv->pbuf_head, + buffer + i, + 1); + if (ret) { + dev_err(&serdev->dev, "chdev write: copy_from_user failed (%d).\n", + ret); + i = len - 1; + } + smp_store_release(&(priv->pbuf_head), + (priv->pbuf_head + 1) & (PBUF_SIZE - 1)); + } else { + dev_warn(&serdev->dev, "chdev write: ring buffer full.\n"); + break; + } + } + len = i; + + schedule_work(&pbuf_work); + dev_info(&serdev->dev, + "Copied %d bytes to FIFO (FIFO free: %d)", + len, CIRC_SPACE(priv->pbuf_head, tail, PBUF_SIZE)); + break; + case PASSIVE: + len = -EBUSY; // TODO: Can I do this? + break; + default: /* Pass-through */ + if (len > 256) { + len = 256; + } + copy_from_user(msg, buffer, len); // TODO: Return value + serdev_device_write_buf(serdev, msg, len); } - copy_from_user(msg, buffer, len); - serdev_device_write_buf(serdev, msg, len); return len; } @@ -337,6 +461,75 @@ __poll_t poll(struct file *filep, struct poll_table_struct *ptab) return 0; } +#define PRINTER_READY_MASK (0b00011111) +int printer_ready_for_data(struct lw35ioc_printer_status printer_status) +{ + return ((printer_status.status & PRINTER_READY_MASK) == PRINTER_READY_MASK); +} + +void pbuf_work_handler(struct work_struct *work) +{ + // TODO: get private data using container_of(): https://linux-kernel-labs.github.io/master/labs/deferred_work.html + struct serdev_device *serdev = static_serdev; // Not cool, but whatever... + struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev); + unsigned long head; + unsigned long tail; + unsigned long lock_flags; + int bsize = 0; + int i; + int locked = 0; + + dev_info(&serdev->dev, + "pbuf_work_handler\n"); + + while (1) { + // TODO: We are the only consumer, so we should be fine without a spinlock, right? + head = smp_load_acquire(&priv->pbuf_head); + tail = priv->pbuf_tail; + bsize = CIRC_CNT(head, tail, PBUF_SIZE); + dev_info(&serdev->dev, "CIRC CNT=%d\n", bsize); + if (bsize == 0) + break; + + spin_lock_irqsave(&printer_status_lock, lock_flags); + locked = 1; + if (!printer_ready_for_data(priv->printer_status)) { + dev_warn(&serdev->dev, "Cannot transmit data to device. Device is not ready.\n"); + break; + } + if (bsize > priv->printer_status.buffer_free) + bsize = priv->printer_status.buffer_free; + if (bsize > CIRC_CNT_TO_END(head, tail, PBUF_SIZE)) + bsize = CIRC_CNT_TO_END(head, tail, PBUF_SIZE); + if (bsize > priv->printer_status.buffer_free - 20) + bsize = priv->printer_status.buffer_free - 20; // TODO: Bug workaround + dev_info(&serdev->dev, "TX %d of %d bytes [%d bytes free on device].\n", + bsize, + CIRC_CNT(head, tail, PBUF_SIZE), + priv->printer_status.buffer_free); + priv->printer_status.buffer_free-=bsize; + spin_unlock_irqrestore(&printer_status_lock, lock_flags); + locked = 0; + if (bsize == 0) { + dev_warn(&serdev->dev, "Cannot transmit data to device. Device buffer is full.\n"); + break; + } + + serdev_device_write_buf(serdev, + priv->pbuf + tail, + bsize); + for (i=0; idev, + "UART TX: %c\n", + priv->pbuf[tail + i]); + } + smp_store_release(&priv->pbuf_tail, + (tail + bsize) & (PBUF_SIZE - 1)); + } + if (locked) + spin_unlock_irqrestore(&printer_status_lock, lock_flags); + wake_up_interruptible(&wq); // There's some space available in the FIFO now, so wake up chardev write +} static struct file_operations fops = @@ -382,7 +575,7 @@ static int lw35ioc_cdev_init(struct serdev_device *serdev) return PTR_ERR(priv->chardev.device); } - dev_err(&serdev->dev, "Device class created correctly\n"); + dev_info(&serdev->dev, "Device class created correctly\n"); return 0; } @@ -414,11 +607,16 @@ static int lw35ioc_probe(struct serdev_device *serdev) priv->uart_buf_rx_rd = priv->uart_buf_rx; priv->uart_buf_rx_wr = priv->uart_buf_rx; + priv->pbuf_tail = 0; + priv->pbuf_head = 0; + + priv->printer_status.buffer_free = 0; + priv->printer_status.status = 0; + 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); /* Register input device */ input_dev = devm_input_allocate_device(&serdev->dev); @@ -440,12 +638,13 @@ static int lw35ioc_probe(struct serdev_device *serdev) goto fail_input; } + serdev_device_set_client_ops(serdev, &lw35ioc_serdev_ops); + ret = serdev_device_open(serdev); if (ret) { dev_err(&serdev->dev, "Unable to open device.\n"); goto fail_serdev; } - speed = serdev_device_set_baudrate(serdev, speed); dev_info(&serdev->dev, "Using baudrate: %u\n", speed); @@ -486,17 +685,26 @@ static void lw35ioc_remove(struct serdev_device *serdev) { struct lw35ioc_priv *priv = serdev_device_get_drvdata(serdev); + dev_info(&serdev->dev, "remove\n"); + dev_info(&serdev->dev, " cdev class: %p\n", priv->chardev.class); + device_destroy(priv->chardev.class, MKDEV(priv->chardev.major_number, 0)); + dev_info(&serdev->dev, "unreg\n"); class_unregister(priv->chardev.class); - class_destroy(priv->chardev.class); + dev_info(&serdev->dev, "destroy: SKIP\n"); + //class_destroy(priv->chardev.class); + dev_info(&serdev->dev, "unreg-dev\n"); unregister_chrdev(priv->chardev.major_number, DEVICE_NAME); + dev_info(&serdev->dev, " gpio rst: %p\n", priv->gpio_rst); + dev_info(&serdev->dev, " serdev: %p\n", priv->serdev); + dev_info(&serdev->dev, " input dev: %p\n", priv->input_dev); lw35ioc_set_hw_reset(serdev, 1); gpiod_put(priv->gpio_rst); serdev_device_close(priv->serdev); input_unregister_device(priv->input_dev); - dev_info(&serdev->dev, "remove\n"); + dev_info(&serdev->dev, "done.\n"); return; }