[linux-2.6-block.git] / drivers / input / serio / ps2mult.c

/*
 * 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(psm);
	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,
};

module_serio_driver(ps2mult_drv);
Commit	Line	Data
fc58d12b DES	1	/*
	2	* TQC PS/2 Multiplexer driver
	3	*
	4	* Copyright (C) 2010 Dmitry Eremin-Solenikov
	5	*
	6	* This program is free software; you can redistribute it and/or modify it
	7	* under the terms of the GNU General Public License version 2 as published by
	8	* the Free Software Foundation.
	9	*/
	10
	11
	12	#include <linux/kernel.h>
	13	#include <linux/slab.h>
	14	#include <linux/module.h>
	15	#include <linux/serio.h>
	16
	17	MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
	18	MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
	19	MODULE_LICENSE("GPL");
	20
	21	#define PS2MULT_KB_SELECTOR 0xA0
	22	#define PS2MULT_MS_SELECTOR 0xA1
	23	#define PS2MULT_ESCAPE 0x7D
	24	#define PS2MULT_BSYNC 0x7E
	25	#define PS2MULT_SESSION_START 0x55
	26	#define PS2MULT_SESSION_END 0x56
	27
	28	struct ps2mult_port {
	29	struct serio *serio;
	30	unsigned char sel;
	31	bool registered;
	32	};
	33
	34	#define PS2MULT_NUM_PORTS 2
	35	#define PS2MULT_KBD_PORT 0
	36	#define PS2MULT_MOUSE_PORT 1
	37
	38	struct ps2mult {
	39	struct serio *mx_serio;
	40	struct ps2mult_port ports[PS2MULT_NUM_PORTS];
	41
	42	spinlock_t lock;
	43	struct ps2mult_port *in_port;
	44	struct ps2mult_port *out_port;
	45	bool escape;
	46	};
	47
	48	/* First MUST come PS2MULT_NUM_PORTS selectors */
	49	static const unsigned char ps2mult_controls[] = {
	50	PS2MULT_KB_SELECTOR, PS2MULT_MS_SELECTOR,
	51	PS2MULT_ESCAPE, PS2MULT_BSYNC,
	52	PS2MULT_SESSION_START, PS2MULT_SESSION_END,
	53	};
	54
	55	static const struct serio_device_id ps2mult_serio_ids[] = {
	56	{
	57	.type = SERIO_RS232,
	58	.proto = SERIO_PS2MULT,
	59	.id = SERIO_ANY,
	60	.extra = SERIO_ANY,
	61	},
	62	{ 0 }
	63	};
	64
65	MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);
66
67	static void ps2mult_select_port(struct ps2mult psm, struct ps2mult_port port)
68	{
69	struct serio *mx_serio = psm->mx_serio;
70
71	serio_write(mx_serio, port->sel);
72	psm->out_port = port;
73	dev_dbg(&mx_serio->dev, "switched to sel %02x\n", port->sel);
74	}
75
76	static int ps2mult_serio_write(struct serio *serio, unsigned char data)
77	{
78	struct serio *mx_port = serio->parent;
79	struct ps2mult *psm = serio_get_drvdata(mx_port);
80	struct ps2mult_port *port = serio->port_data;
81	bool need_escape;
82	unsigned long flags;
83
84	spin_lock_irqsave(&psm->lock, flags);
85
86	if (psm->out_port != port)
87	ps2mult_select_port(psm, port);
88
89	need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls));
90
91	dev_dbg(&serio->dev,
92	"write: %s%02x\n", need_escape ? "ESC " : "", data);
93
94	if (need_escape)
95	serio_write(mx_port, PS2MULT_ESCAPE);
96
97	serio_write(mx_port, data);
98
99	spin_unlock_irqrestore(&psm->lock, flags);
100
101	return 0;
102	}
103
104	static int ps2mult_serio_start(struct serio *serio)
105	{
106	struct ps2mult *psm = serio_get_drvdata(serio->parent);
107	struct ps2mult_port *port = serio->port_data;
108	unsigned long flags;
109
110	spin_lock_irqsave(&psm->lock, flags);
111	port->registered = true;
112	spin_unlock_irqrestore(&psm->lock, flags);
113
114	return 0;
115	}
116
117	static void ps2mult_serio_stop(struct serio *serio)
118	{
119	struct ps2mult *psm = serio_get_drvdata(serio->parent);
120	struct ps2mult_port *port = serio->port_data;
121	unsigned long flags;
122
123	spin_lock_irqsave(&psm->lock, flags);
124	port->registered = false;
125	spin_unlock_irqrestore(&psm->lock, flags);
126	}
127
128	static int ps2mult_create_port(struct ps2mult *psm, int i)
129	{
130	struct serio *mx_serio = psm->mx_serio;
131	struct serio *serio;
132
133	serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
134	if (!serio)
135	return -ENOMEM;
136
137	strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name));
138	snprintf(serio->phys, sizeof(serio->phys),
139	"%s/port%d", mx_serio->phys, i);
140	serio->id.type = SERIO_8042;
141	serio->write = ps2mult_serio_write;
142	serio->start = ps2mult_serio_start;
143	serio->stop = ps2mult_serio_stop;
144	serio->parent = psm->mx_serio;
145	serio->port_data = &psm->ports[i];
146
147	psm->ports[i].serio = serio;
148
149	return 0;
150	}
151
152	static void ps2mult_reset(struct ps2mult *psm)
153	{
154	unsigned long flags;
155
156	spin_lock_irqsave(&psm->lock, flags);
157
158	serio_write(psm->mx_serio, PS2MULT_SESSION_END);
159	serio_write(psm->mx_serio, PS2MULT_SESSION_START);
160
161	ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]);
162
163	spin_unlock_irqrestore(&psm->lock, flags);
164	}
165
166	static int ps2mult_connect(struct serio serio, struct serio_driver drv)
167	{
168	struct ps2mult *psm;
169	int i;
170	int error;
171
172	if (!serio->write)
173	return -EINVAL;
174
175	psm = kzalloc(sizeof(*psm), GFP_KERNEL);
176	if (!psm)
177	return -ENOMEM;
178
179	spin_lock_init(&psm->lock);
180	psm->mx_serio = serio;
181
182	for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
183	psm->ports[i].sel = ps2mult_controls[i];
184	error = ps2mult_create_port(psm, i);
185	if (error)
186	goto err_out;
187	}
188
189	psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT];
190
191	serio_set_drvdata(serio, psm);
192	error = serio_open(serio, drv);
193	if (error)
194	goto err_out;
195
196	ps2mult_reset(psm);
197
198	for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
199	struct serio *s = psm->ports[i].serio;
200
201	dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys);
202	serio_register_port(s);
203	}
204
205	return 0;
206
207	err_out:
208	while (--i >= 0)
209	kfree(psm->ports[i].serio);
0e86eb29	210	kfree(psm);
fc58d12b DES	211	return error;
	212	}
	213
	214	static void ps2mult_disconnect(struct serio *serio)
	215	{
	216	struct ps2mult *psm = serio_get_drvdata(serio);
	217
	218	/* Note that serio core already take care of children ports */
	219	serio_write(serio, PS2MULT_SESSION_END);
	220	serio_close(serio);
	221	kfree(psm);
	222
	223	serio_set_drvdata(serio, NULL);
	224	}
	225
	226	static int ps2mult_reconnect(struct serio *serio)
	227	{
	228	struct ps2mult *psm = serio_get_drvdata(serio);
	229
	230	ps2mult_reset(psm);
	231
	232	return 0;
	233	}
	234
	235	static irqreturn_t ps2mult_interrupt(struct serio *serio,
	236	unsigned char data, unsigned int dfl)
	237	{
	238	struct ps2mult *psm = serio_get_drvdata(serio);
	239	struct ps2mult_port *in_port;
	240	unsigned long flags;
	241
	242	dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl);
	243
	244	spin_lock_irqsave(&psm->lock, flags);
	245
	246	if (psm->escape) {
	247	psm->escape = false;
	248	in_port = psm->in_port;
	249	if (in_port->registered)
	250	serio_interrupt(in_port->serio, data, dfl);
	251	goto out;
	252	}
	253
	254	switch (data) {
	255	case PS2MULT_ESCAPE:
	256	dev_dbg(&serio->dev, "ESCAPE\n");
	257	psm->escape = true;
	258	break;
	259
	260	case PS2MULT_BSYNC:
	261	dev_dbg(&serio->dev, "BSYNC\n");
	262	psm->in_port = psm->out_port;
	263	break;
	264
	265	case PS2MULT_SESSION_START:
	266	dev_dbg(&serio->dev, "SS\n");
	267	break;
	268
	269	case PS2MULT_SESSION_END:
	270	dev_dbg(&serio->dev, "SE\n");
	271	break;
	272
	273	case PS2MULT_KB_SELECTOR:
	274	dev_dbg(&serio->dev, "KB\n");
275	psm->in_port = &psm->ports[PS2MULT_KBD_PORT];
276	break;
277
278	case PS2MULT_MS_SELECTOR:
279	dev_dbg(&serio->dev, "MS\n");
280	psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT];
281	break;
282
283	default:
284	in_port = psm->in_port;
285	if (in_port->registered)
286	serio_interrupt(in_port->serio, data, dfl);
287	break;
288	}
289
290	out:
291	spin_unlock_irqrestore(&psm->lock, flags);
292	return IRQ_HANDLED;
293	}
294
295	static struct serio_driver ps2mult_drv = {
296	.driver = {
297	.name = "ps2mult",
298	},
299	.description = "TQC PS/2 Multiplexer driver",
300	.id_table = ps2mult_serio_ids,
301	.interrupt = ps2mult_interrupt,
302	.connect = ps2mult_connect,
303	.disconnect = ps2mult_disconnect,
304	.reconnect = ps2mult_reconnect,
305	};
306
65ac9f7a	307	module_serio_driver(ps2mult_drv);