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
17MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
18MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
19MODULE_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
28struct 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
38struct 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 */
49static 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
55static 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
65MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);
66
67static 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
76static 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
104static 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
117static 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
128static 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
152static 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
166static 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
207err_out:
208	while (--i >= 0)
209		kfree(psm->ports[i].serio);
210	kfree(psm);
211	return error;
212}
213
214static 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
226static 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
235static 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
295static 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
307static int __init ps2mult_init(void)
308{
309	return serio_register_driver(&ps2mult_drv);
310}
311
312static void __exit ps2mult_exit(void)
313{
314	serio_unregister_driver(&ps2mult_drv);
315}
316
317module_init(ps2mult_init);
318module_exit(ps2mult_exit);
319