ps2mult.c

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