drivers/input/serio/ps2mult.c - kernel/msm-4.19 - Gitiles

 /*
  * TQC PS/2 Multiplexer driver
  *
  * Copyright (C) 2010 Dmitry Eremin-Solenikov
  *
  * This program is free software; you can redistribute it and/or modify it
  * under the terms of the GNU General Public License version 2 as published by
  * the Free Software Foundation.
  */


 #include <linux/kernel.h>
 #include <linux/slab.h>
 #include <linux/module.h>
 #include <linux/serio.h>

 MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
 MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
 MODULE_LICENSE("GPL");

 #define PS2MULT_KB_SELECTOR		0xA0
 #define PS2MULT_MS_SELECTOR		0xA1
 #define PS2MULT_ESCAPE			0x7D
 #define PS2MULT_BSYNC			0x7E
 #define PS2MULT_SESSION_START		0x55
 #define PS2MULT_SESSION_END		0x56

 struct ps2mult_port {
 	struct serio *serio;
 	unsigned char sel;
 	bool registered;
 };

 #define PS2MULT_NUM_PORTS	2
 #define PS2MULT_KBD_PORT	0
 #define PS2MULT_MOUSE_PORT	1

 struct ps2mult {
 	struct serio *mx_serio;
 	struct ps2mult_port ports[PS2MULT_NUM_PORTS];

 	spinlock_t lock;
 	struct ps2mult_port *in_port;
 	struct ps2mult_port *out_port;
 	bool escape;
 };

 /* First MUST come PS2MULT_NUM_PORTS selectors */
 static const unsigned char ps2mult_controls[] = {
 	PS2MULT_KB_SELECTOR, PS2MULT_MS_SELECTOR,
 	PS2MULT_ESCAPE, PS2MULT_BSYNC,
 	PS2MULT_SESSION_START, PS2MULT_SESSION_END,
 };

 static const struct serio_device_id ps2mult_serio_ids[] = {
 	{
 		.type	= SERIO_RS232,
 		.proto	= SERIO_PS2MULT,
 		.id	= SERIO_ANY,
 		.extra	= SERIO_ANY,
 	},
 	{ 0 }
 };

 MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);

 static void ps2mult_select_port(struct ps2mult *psm, struct ps2mult_port *port)
 {
 	struct serio *mx_serio = psm->mx_serio;

 	serio_write(mx_serio, port->sel);
 	psm->out_port = port;
 	dev_dbg(&mx_serio->dev, "switched to sel %02x\n", port->sel);
 }

 static int ps2mult_serio_write(struct serio *serio, unsigned char data)
 {
 	struct serio *mx_port = serio->parent;
 	struct ps2mult *psm = serio_get_drvdata(mx_port);
 	struct ps2mult_port *port = serio->port_data;
 	bool need_escape;
 	unsigned long flags;

 	spin_lock_irqsave(&psm->lock, flags);

 	if (psm->out_port != port)
 		ps2mult_select_port(psm, port);

 	need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls));

 	dev_dbg(&serio->dev,
 		"write: %s%02x\n", need_escape ? "ESC " : "", data);

 	if (need_escape)
 		serio_write(mx_port, PS2MULT_ESCAPE);

 	serio_write(mx_port, data);

 	spin_unlock_irqrestore(&psm->lock, flags);

 	return 0;
 }

 static int ps2mult_serio_start(struct serio *serio)
 {
 	struct ps2mult *psm = serio_get_drvdata(serio->parent);
 	struct ps2mult_port *port = serio->port_data;
 	unsigned long flags;

 	spin_lock_irqsave(&psm->lock, flags);
 	port->registered = true;
 	spin_unlock_irqrestore(&psm->lock, flags);

 	return 0;
 }

 static void ps2mult_serio_stop(struct serio *serio)
 {
 	struct ps2mult *psm = serio_get_drvdata(serio->parent);
 	struct ps2mult_port *port = serio->port_data;
 	unsigned long flags;

 	spin_lock_irqsave(&psm->lock, flags);
 	port->registered = false;
 	spin_unlock_irqrestore(&psm->lock, flags);
 }

 static int ps2mult_create_port(struct ps2mult *psm, int i)
 {
 	struct serio *mx_serio = psm->mx_serio;
 	struct serio *serio;

 	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
 	if (!serio)
 		return -ENOMEM;

 	strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name));
 	snprintf(serio->phys, sizeof(serio->phys),
 		 "%s/port%d", mx_serio->phys, i);
 	serio->id.type = SERIO_8042;
 	serio->write = ps2mult_serio_write;
 	serio->start = ps2mult_serio_start;
 	serio->stop = ps2mult_serio_stop;
 	serio->parent = psm->mx_serio;
 	serio->port_data = &psm->ports[i];

 	psm->ports[i].serio = serio;

 	return 0;
 }

 static void ps2mult_reset(struct ps2mult *psm)
 {
 	unsigned long flags;

 	spin_lock_irqsave(&psm->lock, flags);

 	serio_write(psm->mx_serio, PS2MULT_SESSION_END);
 	serio_write(psm->mx_serio, PS2MULT_SESSION_START);

 	ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]);

 	spin_unlock_irqrestore(&psm->lock, flags);
 }

 static int ps2mult_connect(struct serio *serio, struct serio_driver *drv)
 {
 	struct ps2mult *psm;
 	int i;
 	int error;

 	if (!serio->write)
 		return -EINVAL;

 	psm = kzalloc(sizeof(*psm), GFP_KERNEL);
 	if (!psm)
 		return -ENOMEM;

 	spin_lock_init(&psm->lock);
 	psm->mx_serio = serio;

 	for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
 		psm->ports[i].sel = ps2mult_controls[i];
 		error = ps2mult_create_port(psm, i);
 		if (error)
 			goto err_out;
 	}

 	psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT];

 	serio_set_drvdata(serio, psm);
 	error = serio_open(serio, drv);
 	if (error)
 		goto err_out;

 	ps2mult_reset(psm);

 	for (i = 0; i <  PS2MULT_NUM_PORTS; i++) {
 		struct serio *s = psm->ports[i].serio;

 		dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys);
 		serio_register_port(s);
 	}

 	return 0;

 err_out:
 	while (--i >= 0)
 		kfree(psm->ports[i].serio);
 	kfree(serio);
 	return error;
 }

 static void ps2mult_disconnect(struct serio *serio)
 {
 	struct ps2mult *psm = serio_get_drvdata(serio);

 	/* Note that serio core already take care of children ports */
 	serio_write(serio, PS2MULT_SESSION_END);
 	serio_close(serio);
 	kfree(psm);

 	serio_set_drvdata(serio, NULL);
 }

 static int ps2mult_reconnect(struct serio *serio)
 {
 	struct ps2mult *psm = serio_get_drvdata(serio);

 	ps2mult_reset(psm);

 	return 0;
 }

 static irqreturn_t ps2mult_interrupt(struct serio *serio,
 				     unsigned char data, unsigned int dfl)
 {
 	struct ps2mult *psm = serio_get_drvdata(serio);
 	struct ps2mult_port *in_port;
 	unsigned long flags;

 	dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl);

 	spin_lock_irqsave(&psm->lock, flags);

 	if (psm->escape) {
 		psm->escape = false;
 		in_port = psm->in_port;
 		if (in_port->registered)
 			serio_interrupt(in_port->serio, data, dfl);
 		goto out;
 	}

 	switch (data) {
 	case PS2MULT_ESCAPE:
 		dev_dbg(&serio->dev, "ESCAPE\n");
 		psm->escape = true;
 		break;

 	case PS2MULT_BSYNC:
 		dev_dbg(&serio->dev, "BSYNC\n");
 		psm->in_port = psm->out_port;
 		break;

 	case PS2MULT_SESSION_START:
 		dev_dbg(&serio->dev, "SS\n");
 		break;

 	case PS2MULT_SESSION_END:
 		dev_dbg(&serio->dev, "SE\n");
 		break;

 	case PS2MULT_KB_SELECTOR:
 		dev_dbg(&serio->dev, "KB\n");
 		psm->in_port = &psm->ports[PS2MULT_KBD_PORT];
 		break;

 	case PS2MULT_MS_SELECTOR:
 		dev_dbg(&serio->dev, "MS\n");
 		psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT];
 		break;

 	default:
 		in_port = psm->in_port;
 		if (in_port->registered)
 			serio_interrupt(in_port->serio, data, dfl);
 		break;
 	}

  out:
 	spin_unlock_irqrestore(&psm->lock, flags);
 	return IRQ_HANDLED;
 }

 static struct serio_driver ps2mult_drv = {
 	.driver		= {
 		.name	= "ps2mult",
 	},
 	.description	= "TQC PS/2 Multiplexer driver",
 	.id_table	= ps2mult_serio_ids,
 	.interrupt	= ps2mult_interrupt,
 	.connect	= ps2mult_connect,
 	.disconnect	= ps2mult_disconnect,
 	.reconnect	= ps2mult_reconnect,
 };

 static int __init ps2mult_init(void)
 {
 	return serio_register_driver(&ps2mult_drv);
 }

 static void __exit ps2mult_exit(void)
 {
 	serio_unregister_driver(&ps2mult_drv);
 }

 module_init(ps2mult_init);
 module_exit(ps2mult_exit);
	/*
	* TQC PS/2 Multiplexer driver
	*
	* Copyright (C) 2010 Dmitry Eremin-Solenikov
	*
	* This program is free software; you can redistribute it and/or modify it
	* under the terms of the GNU General Public License version 2 as published by
	* the Free Software Foundation.
	*/


	#include <linux/kernel.h>
	#include <linux/slab.h>
	#include <linux/module.h>
	#include <linux/serio.h>

	MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
	MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
	MODULE_LICENSE("GPL");

	#define PS2MULT_KB_SELECTOR 0xA0
	#define PS2MULT_MS_SELECTOR 0xA1
	#define PS2MULT_ESCAPE 0x7D
	#define PS2MULT_BSYNC 0x7E
	#define PS2MULT_SESSION_START 0x55
	#define PS2MULT_SESSION_END 0x56

	struct ps2mult_port {
	struct serio *serio;
	unsigned char sel;
	bool registered;
	};

	#define PS2MULT_NUM_PORTS 2
	#define PS2MULT_KBD_PORT 0
	#define PS2MULT_MOUSE_PORT 1

	struct ps2mult {
	struct serio *mx_serio;
	struct ps2mult_port ports[PS2MULT_NUM_PORTS];

	spinlock_t lock;
	struct ps2mult_port *in_port;
	struct ps2mult_port *out_port;
	bool escape;
	};

	/* First MUST come PS2MULT_NUM_PORTS selectors */
	static const unsigned char ps2mult_controls[] = {
	PS2MULT_KB_SELECTOR, PS2MULT_MS_SELECTOR,
	PS2MULT_ESCAPE, PS2MULT_BSYNC,
	PS2MULT_SESSION_START, PS2MULT_SESSION_END,
	};

	static const struct serio_device_id ps2mult_serio_ids[] = {
	{
	.type = SERIO_RS232,
	.proto = SERIO_PS2MULT,
	.id = SERIO_ANY,
	.extra = SERIO_ANY,
	},
	{ 0 }
	};

	MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);

	static void ps2mult_select_port(struct ps2mult psm, struct ps2mult_port port)
	{
	struct serio *mx_serio = psm->mx_serio;

	serio_write(mx_serio, port->sel);
	psm->out_port = port;
	dev_dbg(&mx_serio->dev, "switched to sel %02x\n", port->sel);
	}

	static int ps2mult_serio_write(struct serio *serio, unsigned char data)
	{
	struct serio *mx_port = serio->parent;
	struct ps2mult *psm = serio_get_drvdata(mx_port);
	struct ps2mult_port *port = serio->port_data;
	bool need_escape;
	unsigned long flags;

	spin_lock_irqsave(&psm->lock, flags);

	if (psm->out_port != port)
	ps2mult_select_port(psm, port);

	need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls));

	dev_dbg(&serio->dev,
	"write: %s%02x\n", need_escape ? "ESC " : "", data);

	if (need_escape)
	serio_write(mx_port, PS2MULT_ESCAPE);

	serio_write(mx_port, data);

	spin_unlock_irqrestore(&psm->lock, flags);

	return 0;
	}

	static int ps2mult_serio_start(struct serio *serio)
	{
	struct ps2mult *psm = serio_get_drvdata(serio->parent);
	struct ps2mult_port *port = serio->port_data;
	unsigned long flags;

	spin_lock_irqsave(&psm->lock, flags);
	port->registered = true;
	spin_unlock_irqrestore(&psm->lock, flags);

	return 0;
	}

	static void ps2mult_serio_stop(struct serio *serio)
	{
	struct ps2mult *psm = serio_get_drvdata(serio->parent);
	struct ps2mult_port *port = serio->port_data;
	unsigned long flags;

	spin_lock_irqsave(&psm->lock, flags);
	port->registered = false;
	spin_unlock_irqrestore(&psm->lock, flags);
	}

	static int ps2mult_create_port(struct ps2mult *psm, int i)
	{
	struct serio *mx_serio = psm->mx_serio;
	struct serio *serio;

	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
	if (!serio)
	return -ENOMEM;

	strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name));
	snprintf(serio->phys, sizeof(serio->phys),
	"%s/port%d", mx_serio->phys, i);
	serio->id.type = SERIO_8042;
	serio->write = ps2mult_serio_write;
	serio->start = ps2mult_serio_start;
	serio->stop = ps2mult_serio_stop;
	serio->parent = psm->mx_serio;
	serio->port_data = &psm->ports[i];

	psm->ports[i].serio = serio;

	return 0;
	}

	static void ps2mult_reset(struct ps2mult *psm)
	{
	unsigned long flags;

	spin_lock_irqsave(&psm->lock, flags);

	serio_write(psm->mx_serio, PS2MULT_SESSION_END);
	serio_write(psm->mx_serio, PS2MULT_SESSION_START);

	ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]);

	spin_unlock_irqrestore(&psm->lock, flags);
	}

	static int ps2mult_connect(struct serio serio, struct serio_driver drv)
	{
	struct ps2mult *psm;
	int i;
	int error;

	if (!serio->write)
	return -EINVAL;

	psm = kzalloc(sizeof(*psm), GFP_KERNEL);
	if (!psm)
	return -ENOMEM;

	spin_lock_init(&psm->lock);
	psm->mx_serio = serio;

	for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
	psm->ports[i].sel = ps2mult_controls[i];
	error = ps2mult_create_port(psm, i);
	if (error)
	goto err_out;
	}

	psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT];

	serio_set_drvdata(serio, psm);
	error = serio_open(serio, drv);
	if (error)
	goto err_out;

	ps2mult_reset(psm);

	for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
	struct serio *s = psm->ports[i].serio;

	dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys);
	serio_register_port(s);
	}

	return 0;

	err_out:
	while (--i >= 0)
	kfree(psm->ports[i].serio);
	kfree(serio);
	return error;
	}

	static void ps2mult_disconnect(struct serio *serio)
	{
	struct ps2mult *psm = serio_get_drvdata(serio);

	/* Note that serio core already take care of children ports */
	serio_write(serio, PS2MULT_SESSION_END);
	serio_close(serio);
	kfree(psm);

	serio_set_drvdata(serio, NULL);
	}

	static int ps2mult_reconnect(struct serio *serio)
	{
	struct ps2mult *psm = serio_get_drvdata(serio);

	ps2mult_reset(psm);

	return 0;
	}

	static irqreturn_t ps2mult_interrupt(struct serio *serio,
	unsigned char data, unsigned int dfl)
	{
	struct ps2mult *psm = serio_get_drvdata(serio);
	struct ps2mult_port *in_port;
	unsigned long flags;

	dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl);

	spin_lock_irqsave(&psm->lock, flags);

	if (psm->escape) {
	psm->escape = false;
	in_port = psm->in_port;
	if (in_port->registered)
	serio_interrupt(in_port->serio, data, dfl);
	goto out;
	}

	switch (data) {
	case PS2MULT_ESCAPE:
	dev_dbg(&serio->dev, "ESCAPE\n");
	psm->escape = true;
	break;

	case PS2MULT_BSYNC:
	dev_dbg(&serio->dev, "BSYNC\n");
	psm->in_port = psm->out_port;
	break;

	case PS2MULT_SESSION_START:
	dev_dbg(&serio->dev, "SS\n");
	break;

	case PS2MULT_SESSION_END:
	dev_dbg(&serio->dev, "SE\n");
	break;

	case PS2MULT_KB_SELECTOR:
	dev_dbg(&serio->dev, "KB\n");
	psm->in_port = &psm->ports[PS2MULT_KBD_PORT];
	break;

	case PS2MULT_MS_SELECTOR:
	dev_dbg(&serio->dev, "MS\n");
	psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT];
	break;

	default:
	in_port = psm->in_port;
	if (in_port->registered)
	serio_interrupt(in_port->serio, data, dfl);
	break;
	}

	out:
	spin_unlock_irqrestore(&psm->lock, flags);
	return IRQ_HANDLED;
	}

	static struct serio_driver ps2mult_drv = {
	.driver = {
	.name = "ps2mult",
	},
	.description = "TQC PS/2 Multiplexer driver",
	.id_table = ps2mult_serio_ids,
	.interrupt = ps2mult_interrupt,
	.connect = ps2mult_connect,
	.disconnect = ps2mult_disconnect,
	.reconnect = ps2mult_reconnect,
	};

	static int __init ps2mult_init(void)
	{
	return serio_register_driver(&ps2mult_drv);
	}

	static void __exit ps2mult_exit(void)
	{
	serio_unregister_driver(&ps2mult_drv);
	}

	module_init(ps2mult_init);
	module_exit(ps2mult_exit);