[PATCH] Input: serio_raw - return proper result when serio_raw_read fails

@@ -164,7 +164,8 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
 	char uninitialized_var(c);
-	ssize_t retval = 0;
+	ssize_t read = 0;
+	int retval;
 
 	if (serio_raw->dead)
 		return -ENODEV;

@@ -180,13 +181,15 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 	if (serio_raw->dead)
 		return -ENODEV;
 
-	while (retval < count && serio_raw_fetch_byte(serio_raw, &c)) {
-		if (put_user(c, buffer++))
-			return -EFAULT;
-		retval++;
+	while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
+		if (put_user(c, buffer++)) {
+			retval = -EFAULT;
+			break;
+		}
+		read++;
 	}
 
-	return retval;
+	return read ?: retval;
 }
 
 static ssize_t serio_raw_write(struct file *file, const char __user *buffer,

@@ -164,7 +164,8 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
 	char uninitialized_var(c);
-	ssize_t retval = 0;
+	ssize_t read = 0;
+	int retval;
 
 	if (serio_raw->dead)
 		return -ENODEV;

@@ -180,13 +181,15 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 	if (serio_raw->dead)
 		return -ENODEV;
 
-	while (retval < count && serio_raw_fetch_byte(serio_raw, &c)) {
-		if (put_user(c, buffer++))
-			return -EFAULT;
-		retval++;
+	while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
+		if (put_user(c, buffer++)) {
+			retval = -EFAULT;
+			break;
+		}
+		read++;
 	}
 
-	return retval;
+	return read ?: retval;
 }
 
 static ssize_t serio_raw_write(struct file *file, const char __user *buffer,

@@ -29,15 +29,19 @@ MODULE_DESCRIPTION(DRIVER_DESC);
 MODULE_LICENSE("GPL");
 
 #define SERIO_RAW_QUEUE_LEN	64
-struct serio_raw {
+struct queue {
 	unsigned char queue[SERIO_RAW_QUEUE_LEN];
 	unsigned int tail, head;
+	wait_queue_head_t wait;
+};
+
+struct serio_raw {
+	struct queue queue;
 
 	char name[16];
 	struct kref kref;
 	struct serio *serio;
 	struct miscdevice dev;
-	wait_queue_head_t wait;
 	struct list_head client_list;
 	struct list_head node;
 	bool dead;

@@ -53,6 +57,105 @@ static DEFINE_MUTEX(serio_raw_mutex);
 static LIST_HEAD(serio_raw_list);
 
 /*********************************************************************
+ *             Interface with queue                                  *
+ *********************************************************************/
+
+static void queue_init(struct queue *queue)
+{
+	init_waitqueue_head(&queue->wait);
+}
+
+static void queue_clear(struct queue *queue)
+{
+	queue->head = queue->tail = 0;
+}
+
+static bool queue_available(struct queue *queue)
+{
+	return queue->head != queue->tail;
+}
+
+static void queue_wakeup(struct queue *queue)
+{
+	wake_up_interruptible(&queue->wait);
+}
+
+static bool queue_fetch_byte(struct queue *queue, char *c)
+{
+	bool available;
+
+	available = queue_available(queue);
+	if (available) {
+		*c = queue->queue[queue->tail];
+		queue->tail = (queue->tail + 1) % SERIO_RAW_QUEUE_LEN;
+	}
+
+	return available;
+}
+
+static ssize_t queue_read(struct queue *queue,
+		char __user *buffer, size_t count, bool *dead, bool nonblock,
+		bool (*fetch_byte)(struct queue *, char *))
+{
+	char uninitialized_var(c);
+	ssize_t read = 0;
+	int retval;
+
+	if (*dead)
+		return -ENODEV;
+
+	if (!queue_available(queue) && nonblock)
+		return -EAGAIN;
+
+	retval = wait_event_interruptible(queue->wait,
+			queue_available(queue) || *dead);
+	if (retval)
+		return retval;
+
+	if (*dead)
+		return -ENODEV;
+
+	while (read < count && fetch_byte(queue, &c)) {
+		if (put_user(c, buffer++)) {
+			retval = -EFAULT;
+			break;
+		}
+		read++;
+	}
+
+	return read ?: retval;
+}
+
+static bool queue_write_byte(struct queue *queue, char c)
+{
+	unsigned int head;
+	bool may_write;
+
+	queue->queue[queue->head] = c;
+	head = (queue->head + 1) % SERIO_RAW_QUEUE_LEN;
+	may_write = head != queue->tail;
+	if (may_write)
+		queue->head = head;
+
+	return may_write;
+}
+
+static unsigned int queue_poll(struct queue *queue, struct file *file,
+		poll_table *wait, bool *dead)
+{
+	unsigned int mask;
+
+	poll_wait(file, &queue->wait, wait);
+
+	mask = *dead ? POLLHUP | POLLERR : POLLOUT | POLLWRNORM;
+	if (queue_available(queue))
+		mask |= POLLIN | POLLRDNORM;
+
+	return mask;
+}
+
+
+/*********************************************************************
  *             Interface with userspace (file operations)            *
  *********************************************************************/
 

@@ -141,21 +244,17 @@ static int serio_raw_release(struct inode *inode, struct file *file)
 	return 0;
 }
 
-static bool serio_raw_fetch_byte(struct serio_raw *serio_raw, char *c)
+static bool serio_raw_fetch_byte(struct queue *queue, char *c)
 {
-	bool empty;
+	struct serio_raw *serio_raw;
+	bool available;
 
+	serio_raw = container_of(queue, struct serio_raw, queue);
 	serio_pause_rx(serio_raw->serio);
-
-	empty = serio_raw->head == serio_raw->tail;
-	if (!empty) {
-		*c = serio_raw->queue[serio_raw->tail];
-		serio_raw->tail = (serio_raw->tail + 1) % SERIO_RAW_QUEUE_LEN;
-	}
-
+	available = queue_fetch_byte(queue, c);
 	serio_continue_rx(serio_raw->serio);
 
-	return !empty;
+	return available;
 }
 
 static ssize_t serio_raw_read(struct file *file, char __user *buffer,

@@ -163,33 +262,11 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 {
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
-	char uninitialized_var(c);
-	ssize_t read = 0;
-	int retval;
-
-	if (serio_raw->dead)
-		return -ENODEV;
-
-	if (serio_raw->head == serio_raw->tail && (file->f_flags & O_NONBLOCK))
-		return -EAGAIN;
-
-	retval = wait_event_interruptible(serio_raw->wait,
-			serio_raw->head != serio_raw->tail || serio_raw->dead);
-	if (retval)
-		return retval;
-
-	if (serio_raw->dead)
-		return -ENODEV;
+	struct queue *queue = &serio_raw->queue;
 
-	while (read < count && serio_raw_fetch_byte(serio_raw, &c)) {
-		if (put_user(c, buffer++)) {
-			retval = -EFAULT;
-			break;
-		}
-		read++;
-	}
-
-	return read ?: retval;
+	return queue_read(queue, buffer, count,
+			&serio_raw->dead, file->f_flags & O_NONBLOCK,
+			serio_raw_fetch_byte);
 }
 
 static ssize_t serio_raw_write(struct file *file, const char __user *buffer,

@@ -234,15 +311,9 @@ static unsigned int serio_raw_poll(struct file *file, poll_table *wait)
 {
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
-	unsigned int mask;
-
-	poll_wait(file, &serio_raw->wait, wait);
+	struct queue *queue = &serio_raw->queue;
 
-	mask = serio_raw->dead ? POLLHUP | POLLERR : POLLOUT | POLLWRNORM;
-	if (serio_raw->head != serio_raw->tail)
-		mask |= POLLIN | POLLRDNORM;
-
-	return mask;
+	return queue_poll(queue, file, wait, &serio_raw->dead);
 }
 
 static const struct file_operations serio_raw_fops = {

@@ -266,16 +337,12 @@ static irqreturn_t serio_raw_interrupt(struct serio *serio, unsigned char data,
 {
 	struct serio_raw *serio_raw = serio_get_drvdata(serio);
 	struct serio_raw_client *client;
-	unsigned int head = serio_raw->head;
 
 	/* we are holding serio->lock here so we are protected */
-	serio_raw->queue[head] = data;
-	head = (head + 1) % SERIO_RAW_QUEUE_LEN;
-	if (likely(head != serio_raw->tail)) {
-		serio_raw->head = head;
+	if (queue_write_byte(&serio_raw->queue, data)) {
 		list_for_each_entry(client, &serio_raw->client_list, node)
 			kill_fasync(&client->fasync, SIGIO, POLL_IN);
-		wake_up_interruptible(&serio_raw->wait);
+		queue_wakeup(&serio_raw->queue);
 	}
 
 	return IRQ_HANDLED;

@@ -297,7 +364,7 @@ static int serio_raw_connect(struct serio *serio, struct serio_driver *drv)
 		 "serio_raw%ld", (long)atomic_inc_return(&serio_raw_no) - 1);
 	kref_init(&serio_raw->kref);
 	INIT_LIST_HEAD(&serio_raw->client_list);
-	init_waitqueue_head(&serio_raw->wait);
+	queue_init(&serio_raw->queue);
 
 	serio_raw->serio = serio;
 	get_device(&serio->dev);

@@ -378,7 +445,7 @@ static void serio_raw_hangup(struct serio_raw *serio_raw)
 		kill_fasync(&client->fasync, SIGIO, POLL_HUP);
 	serio_continue_rx(serio_raw->serio);
 
-	wake_up_interruptible(&serio_raw->wait);
+	queue_wakeup(&serio_raw->queue);
 }
 
 

@@ -269,12 +269,12 @@ static ssize_t serio_raw_read(struct file *file, char __user *buffer,
 			serio_raw_fetch_byte);
 }
 
-static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
-			       size_t count, loff_t *ppos)
+static ssize_t serio_raw_write_mainloop(struct serio_raw *serio_raw,
+		const char __user *buffer, size_t count,
+		bool write_to_dev, struct queue *queue)
 {
-	struct serio_raw_client *client = file->private_data;
-	struct serio_raw *serio_raw = client->serio_raw;
 	ssize_t written = 0;
+	bool wakeup = false;
 	int retval;
 	unsigned char c;
 

@@ -293,20 +293,33 @@ static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
 	while (count--) {
 		if (get_user(c, buffer++)) {
 			retval = -EFAULT;
-			goto out;
+			break;
 		}
-		if (serio_write(serio_raw->serio, c)) {
+		if (write_to_dev && serio_write(serio_raw->serio, c)) {
 			retval = -EIO;
-			goto out;
+			break;
 		}
+		if (queue)
+			wakeup |= queue_write_byte(queue, c);
 		written++;
 	}
+	if (queue && wakeup)
+		queue_wakeup(queue);
 
 out:
 	mutex_unlock(&serio_raw_mutex);
 	return written ?: retval;
 }
 
+static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
+			       size_t count, loff_t *ppos)
+{
+	struct serio_raw_client *client = file->private_data;
+	struct serio_raw *serio_raw = client->serio_raw;
+
+	return serio_raw_write_mainloop(serio_raw, buffer, count, true, NULL);
+}
+
 static unsigned int serio_raw_poll(struct file *file, poll_table *wait)
 {
 	struct serio_raw_client *client = file->private_data;

@@ -37,7 +37,7 @@ struct queue {
 };
 
 struct serio_raw {
-	struct queue queue;
+	struct queue queue, debug_user, debug_device;
 
 	char name[16];
 	struct kref kref;

@@ -46,6 +46,7 @@ struct serio_raw {
 	struct list_head client_list;
 	struct list_head node;
 	bool dead;
+	bool debug_user_opened, debug_device_opened;
 	u32 replay;  /* not bool because debugfs_create_bool() takes u32 */
 
 	struct dentry *dentry;

@@ -320,8 +321,11 @@ static ssize_t serio_raw_write(struct file *file, const char __user *buffer,
 {
 	struct serio_raw_client *client = file->private_data;
 	struct serio_raw *serio_raw = client->serio_raw;
+	struct queue *queue = serio_raw->debug_user_opened ?
+			&serio_raw->debug_user : NULL;
 
-	return serio_raw_write_mainloop(serio_raw, buffer, count, true, NULL);
+	return serio_raw_write_mainloop(serio_raw, buffer, count,
+			!serio_raw->replay, queue);
 }
 
 static unsigned int serio_raw_poll(struct file *file, poll_table *wait)

@@ -349,10 +353,144 @@ static const struct file_operations serio_raw_fops = {
  *             Interface with debugfs (file operations)              *
  *********************************************************************/
 
+static int debug_user_open(struct inode *inode, struct file *file)
+{
+	struct serio_raw *serio_raw = inode->i_private;
+
+	file->private_data = serio_raw;
+	queue_clear(&serio_raw->debug_user);
+	serio_raw->debug_user_opened = true;
+	kref_get(&serio_raw->kref);
+
+	return 0;
+}
+
+static int debug_device_open(struct inode *inode, struct file *file)
+{
+	struct serio_raw *serio_raw = inode->i_private;
+
+	file->private_data = serio_raw;
+	queue_clear(&serio_raw->debug_device);
+	serio_raw->debug_device_opened = true;
+	kref_get(&serio_raw->kref);
+
+	return 0;
+}
+
+static int debug_user_release(struct inode *inode, struct file *file)
+{
+	struct serio_raw *serio_raw = file->private_data;
+
+	file->private_data = NULL;
+	serio_raw->debug_user_opened = false;
+	kref_put(&serio_raw->kref, serio_raw_free);
+
+	return 0;
+}
+
+static int debug_device_release(struct inode *inode, struct file *file)
+{
+	struct serio_raw *serio_raw = file->private_data;
+
+	file->private_data = NULL;
+	serio_raw->debug_device_opened = false;
+	kref_put(&serio_raw->kref, serio_raw_free);
+
+	return 0;
+}
+
+static ssize_t debug_user_read(struct file *file,
+		char __user *buffer, size_t count, loff_t *ppos)
+{
+	struct serio_raw *serio_raw = file->private_data;
+	struct queue *queue = &serio_raw->debug_user;
+
+	return queue_read(queue, buffer, count,
+			&serio_raw->dead, file->f_flags & O_NONBLOCK,
+			queue_fetch_byte);
+}
+
+static ssize_t debug_device_read(struct file *file,
+		char __user *buffer, size_t count, loff_t *ppos)
+{
+	struct serio_raw *serio_raw = file->private_data;
+	struct queue *queue = &serio_raw->debug_device;
+
+	return queue_read(queue, buffer, count,
+			&serio_raw->dead, file->f_flags & O_NONBLOCK,
+			queue_fetch_byte);
+}
+
+static ssize_t debug_user_write(struct file *file,
+		const char __user *buffer, size_t count, loff_t *ppos)
+{
+	struct serio_raw *serio_raw = file->private_data;
+	struct serio_raw_client *client;
+	size_t written = 0;
+
+	if (!serio_raw->replay)
+		return -EIO;
+
+	serio_pause_rx(serio_raw->serio);
+
+	for (written = 0; written < count; written++)
+		if (!queue_write_byte(&serio_raw->queue, buffer[written]))
+			break;
+	if (written) {
+		list_for_each_entry(client, &serio_raw->client_list, node)
+			kill_fasync(&client->fasync, SIGIO, POLL_IN);
+		queue_wakeup(&serio_raw->queue);
+	}
+
+	serio_continue_rx(serio_raw->serio);
+	return written ?: -EIO;
+}
+
+static ssize_t debug_device_write(struct file *file,
+		const char __user *buffer, size_t count, loff_t *ppos)
+{
+	struct serio_raw *serio_raw = file->private_data;
+
+	if (!serio_raw->replay)
+		return -EIO;
+
+	return serio_raw_write_mainloop(serio_raw, buffer, count, true, NULL);
+}
+
+static unsigned int debug_user_poll(struct file *file, poll_table *wait)
+{
+	struct serio_raw *serio_raw = file->private_data;
+	struct queue *queue = &serio_raw->debug_user;
+
+	return queue_poll(queue, file, wait, &serio_raw->dead);
+}
+
+static unsigned int debug_device_poll(struct file *file, poll_table *wait)
+{
+	struct serio_raw *serio_raw = file->private_data;
+	struct queue *queue = &serio_raw->debug_device;
+
+	return queue_poll(queue, file, wait, &serio_raw->dead);
+}
+
 static const struct file_operations debug_user_fops = {
+	.owner		= THIS_MODULE,
+	.open		= debug_user_open,
+	.release	= debug_user_release,
+	.read		= debug_user_read,
+	.write		= debug_user_write,
+	.poll		= debug_user_poll,
+	.llseek		= noop_llseek,
 };
 
 static const struct file_operations debug_device_fops = {
+	.owner		= THIS_MODULE,
+	.open		= debug_device_open,
+	.release	= debug_device_release,
+	.read		= debug_device_read,
+	.write		= debug_device_write,
+	.poll		= debug_device_poll,
+	.llseek		= noop_llseek,
 };
 
 

@@ -367,7 +505,12 @@ static irqreturn_t serio_raw_interrupt(struct serio *serio, unsigned char data,
 	struct serio_raw_client *client;
 
 	/* we are holding serio->lock here so we are protected */
-	if (queue_write_byte(&serio_raw->queue, data)) {
+
+	if (serio_raw->debug_device_opened &&
+			queue_write_byte(&serio_raw->debug_device, data))
+		queue_wakeup(&serio_raw->debug_device);
+
+	if (!serio_raw->replay && queue_write_byte(&serio_raw->queue, data)) {
 		list_for_each_entry(client, &serio_raw->client_list, node)
 			kill_fasync(&client->fasync, SIGIO, POLL_IN);
 		queue_wakeup(&serio_raw->queue);

@@ -394,6 +537,9 @@ static int serio_raw_debug_init(struct serio_raw *serio_raw)
 				&debug_device_fops))
 		goto err;
 
+	queue_init(&serio_raw->debug_user);
+	queue_init(&serio_raw->debug_device);
+
 	return 0;
 
 err:

@@ -21,6 +21,7 @@
 #include <linux/miscdevice.h>
 #include <linux/wait.h>
 #include <linux/mutex.h>
+#include <linux/debugfs.h>
 
 #define DRIVER_DESC	"Raw serio driver"
 

@@ -45,6 +46,9 @@ struct serio_raw {
 	struct list_head client_list;
 	struct list_head node;
 	bool dead;
+	u32 replay;  /* not bool because debugfs_create_bool() takes u32 */
+
+	struct dentry *dentry;
 };
 
 struct serio_raw_client {

@@ -342,6 +346,17 @@ static const struct file_operations serio_raw_fops = {
 
 
 /*********************************************************************
+ *             Interface with debugfs (file operations)              *
+ *********************************************************************/
+
+static const struct file_operations debug_user_fops = {
+};
+
+static const struct file_operations debug_device_fops = {
+};
+
+
+/*********************************************************************
  *                   Interface with serio port                       *
  *********************************************************************/
 

@@ -361,6 +376,36 @@ static irqreturn_t serio_raw_interrupt(struct serio *serio, unsigned char data,
 	return IRQ_HANDLED;
 }
 
+static int serio_raw_debug_init(struct serio_raw *serio_raw)
+{
+	const mode_t mode = S_IRUSR | S_IWUSR;
+
+	serio_raw->dentry = debugfs_create_dir(serio_raw->name, NULL);
+	if (!serio_raw->dentry)
+		return -ENOENT;
+
+	if (!debugfs_create_bool("replay", mode, serio_raw->dentry,
+				&serio_raw->replay))
+		goto err;
+	if (!debugfs_create_file("user", mode, serio_raw->dentry, serio_raw,
+				&debug_user_fops))
+		goto err;
+	if (!debugfs_create_file("device", mode, serio_raw->dentry, serio_raw,
+				&debug_device_fops))
+		goto err;
+
+	return 0;
+
+err:
+	debugfs_remove_recursive(serio_raw->dentry);
+	return -ENOENT;
+}
+
+static void serio_raw_debug_cleanup(struct serio_raw *serio_raw)
+{
+	debugfs_remove_recursive(serio_raw->dentry);
+}
+
 static int serio_raw_connect(struct serio *serio, struct serio_driver *drv)
 {
 	static atomic_t serio_raw_no = ATOMIC_INIT(0);

@@ -413,6 +458,10 @@ static int serio_raw_connect(struct serio *serio, struct serio_driver *drv)
 		goto err_unlink;
 	}
 
+	if (serio_raw_debug_init(serio_raw))
+		dev_warn(&serio->dev, "failed to register debugfs for %s\n",
+				serio->phys);
+
 	dev_info(&serio->dev, "raw access enabled on %s (%s, minor %d)\n",
 		 serio->phys, serio_raw->name, serio_raw->dev.minor);
 	return 0;

@@ -475,6 +524,8 @@ static void serio_raw_disconnect(struct serio *serio)
 
 	serio_raw_hangup(serio_raw);
 
+	serio_raw_debug_cleanup(serio_raw);
+
 	serio_close(serio);
 	kref_put(&serio_raw->kref, serio_raw_free);