Loading...
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);
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
307module_serio_driver(ps2mult_drv);