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 | |
14 | MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>" ); |
15 | MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver" ); |
16 | MODULE_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 | |
25 | struct 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 | |
35 | struct 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 */ |
46 | static 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 | |
52 | static 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 | |
62 | MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids); |
63 | |
64 | static void ps2mult_select_port(struct ps2mult *psm, struct ps2mult_port *port) |
65 | { |
66 | struct serio *mx_serio = psm->mx_serio; |
67 | |
68 | serio_write(serio: mx_serio, data: port->sel); |
69 | psm->out_port = port; |
70 | dev_dbg(&mx_serio->dev, "switched to sel %02x\n" , port->sel); |
71 | } |
72 | |
73 | static 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(serio: mx_port); |
77 | struct ps2mult_port *port = serio->port_data; |
78 | bool need_escape; |
79 | unsigned long flags; |
80 | |
81 | spin_lock_irqsave(&psm->lock, flags); |
82 | |
83 | if (psm->out_port != port) |
84 | ps2mult_select_port(psm, port); |
85 | |
86 | need_escape = memchr(p: ps2mult_controls, c: data, size: sizeof(ps2mult_controls)); |
87 | |
88 | dev_dbg(&serio->dev, |
89 | "write: %s%02x\n" , need_escape ? "ESC " : "" , data); |
90 | |
91 | if (need_escape) |
92 | serio_write(serio: mx_port, PS2MULT_ESCAPE); |
93 | |
94 | serio_write(serio: mx_port, data); |
95 | |
96 | spin_unlock_irqrestore(lock: &psm->lock, flags); |
97 | |
98 | return 0; |
99 | } |
100 | |
101 | static int ps2mult_serio_start(struct serio *serio) |
102 | { |
103 | struct ps2mult *psm = serio_get_drvdata(serio: serio->parent); |
104 | struct ps2mult_port *port = serio->port_data; |
105 | unsigned long flags; |
106 | |
107 | spin_lock_irqsave(&psm->lock, flags); |
108 | port->registered = true; |
109 | spin_unlock_irqrestore(lock: &psm->lock, flags); |
110 | |
111 | return 0; |
112 | } |
113 | |
114 | static void ps2mult_serio_stop(struct serio *serio) |
115 | { |
116 | struct ps2mult *psm = serio_get_drvdata(serio: serio->parent); |
117 | struct ps2mult_port *port = serio->port_data; |
118 | unsigned long flags; |
119 | |
120 | spin_lock_irqsave(&psm->lock, flags); |
121 | port->registered = false; |
122 | spin_unlock_irqrestore(lock: &psm->lock, flags); |
123 | } |
124 | |
125 | static int ps2mult_create_port(struct ps2mult *psm, int i) |
126 | { |
127 | struct serio *mx_serio = psm->mx_serio; |
128 | struct serio *serio; |
129 | |
130 | serio = kzalloc(size: sizeof(struct serio), GFP_KERNEL); |
131 | if (!serio) |
132 | return -ENOMEM; |
133 | |
134 | strscpy(p: serio->name, q: "TQC PS/2 Multiplexer" , size: sizeof(serio->name)); |
135 | snprintf(buf: serio->phys, size: sizeof(serio->phys), |
136 | fmt: "%s/port%d" , mx_serio->phys, i); |
137 | serio->id.type = SERIO_8042; |
138 | serio->write = ps2mult_serio_write; |
139 | serio->start = ps2mult_serio_start; |
140 | serio->stop = ps2mult_serio_stop; |
141 | serio->parent = psm->mx_serio; |
142 | serio->port_data = &psm->ports[i]; |
143 | |
144 | psm->ports[i].serio = serio; |
145 | |
146 | return 0; |
147 | } |
148 | |
149 | static void ps2mult_reset(struct ps2mult *psm) |
150 | { |
151 | unsigned long flags; |
152 | |
153 | spin_lock_irqsave(&psm->lock, flags); |
154 | |
155 | serio_write(serio: psm->mx_serio, PS2MULT_SESSION_END); |
156 | serio_write(serio: psm->mx_serio, PS2MULT_SESSION_START); |
157 | |
158 | ps2mult_select_port(psm, port: &psm->ports[PS2MULT_KBD_PORT]); |
159 | |
160 | spin_unlock_irqrestore(lock: &psm->lock, flags); |
161 | } |
162 | |
163 | static int ps2mult_connect(struct serio *serio, struct serio_driver *drv) |
164 | { |
165 | struct ps2mult *psm; |
166 | int i; |
167 | int error; |
168 | |
169 | if (!serio->write) |
170 | return -EINVAL; |
171 | |
172 | psm = kzalloc(size: sizeof(*psm), GFP_KERNEL); |
173 | if (!psm) |
174 | return -ENOMEM; |
175 | |
176 | spin_lock_init(&psm->lock); |
177 | psm->mx_serio = serio; |
178 | |
179 | for (i = 0; i < PS2MULT_NUM_PORTS; i++) { |
180 | psm->ports[i].sel = ps2mult_controls[i]; |
181 | error = ps2mult_create_port(psm, i); |
182 | if (error) |
183 | goto err_out; |
184 | } |
185 | |
186 | psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT]; |
187 | |
188 | serio_set_drvdata(serio, data: psm); |
189 | error = serio_open(serio, drv); |
190 | if (error) |
191 | goto err_out; |
192 | |
193 | ps2mult_reset(psm); |
194 | |
195 | for (i = 0; i < PS2MULT_NUM_PORTS; i++) { |
196 | struct serio *s = psm->ports[i].serio; |
197 | |
198 | dev_info(&serio->dev, "%s port at %s\n" , s->name, serio->phys); |
199 | serio_register_port(s); |
200 | } |
201 | |
202 | return 0; |
203 | |
204 | err_out: |
205 | while (--i >= 0) |
206 | kfree(objp: psm->ports[i].serio); |
207 | kfree(objp: psm); |
208 | return error; |
209 | } |
210 | |
211 | static void ps2mult_disconnect(struct serio *serio) |
212 | { |
213 | struct ps2mult *psm = serio_get_drvdata(serio); |
214 | |
215 | /* Note that serio core already take care of children ports */ |
216 | serio_write(serio, PS2MULT_SESSION_END); |
217 | serio_close(serio); |
218 | kfree(objp: psm); |
219 | |
220 | serio_set_drvdata(serio, NULL); |
221 | } |
222 | |
223 | static int ps2mult_reconnect(struct serio *serio) |
224 | { |
225 | struct ps2mult *psm = serio_get_drvdata(serio); |
226 | |
227 | ps2mult_reset(psm); |
228 | |
229 | return 0; |
230 | } |
231 | |
232 | static irqreturn_t ps2mult_interrupt(struct serio *serio, |
233 | unsigned char data, unsigned int dfl) |
234 | { |
235 | struct ps2mult *psm = serio_get_drvdata(serio); |
236 | struct ps2mult_port *in_port; |
237 | unsigned long flags; |
238 | |
239 | dev_dbg(&serio->dev, "Received %02x flags %02x\n" , data, dfl); |
240 | |
241 | spin_lock_irqsave(&psm->lock, flags); |
242 | |
243 | if (psm->escape) { |
244 | psm->escape = false; |
245 | in_port = psm->in_port; |
246 | if (in_port->registered) |
247 | serio_interrupt(serio: in_port->serio, data, flags: dfl); |
248 | goto out; |
249 | } |
250 | |
251 | switch (data) { |
252 | case PS2MULT_ESCAPE: |
253 | dev_dbg(&serio->dev, "ESCAPE\n" ); |
254 | psm->escape = true; |
255 | break; |
256 | |
257 | case PS2MULT_BSYNC: |
258 | dev_dbg(&serio->dev, "BSYNC\n" ); |
259 | psm->in_port = psm->out_port; |
260 | break; |
261 | |
262 | case PS2MULT_SESSION_START: |
263 | dev_dbg(&serio->dev, "SS\n" ); |
264 | break; |
265 | |
266 | case PS2MULT_SESSION_END: |
267 | dev_dbg(&serio->dev, "SE\n" ); |
268 | break; |
269 | |
270 | case PS2MULT_KB_SELECTOR: |
271 | dev_dbg(&serio->dev, "KB\n" ); |
272 | psm->in_port = &psm->ports[PS2MULT_KBD_PORT]; |
273 | break; |
274 | |
275 | case PS2MULT_MS_SELECTOR: |
276 | dev_dbg(&serio->dev, "MS\n" ); |
277 | psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT]; |
278 | break; |
279 | |
280 | default: |
281 | in_port = psm->in_port; |
282 | if (in_port->registered) |
283 | serio_interrupt(serio: in_port->serio, data, flags: dfl); |
284 | break; |
285 | } |
286 | |
287 | out: |
288 | spin_unlock_irqrestore(lock: &psm->lock, flags); |
289 | return IRQ_HANDLED; |
290 | } |
291 | |
292 | static struct serio_driver ps2mult_drv = { |
293 | .driver = { |
294 | .name = "ps2mult" , |
295 | }, |
296 | .description = "TQC PS/2 Multiplexer driver" , |
297 | .id_table = ps2mult_serio_ids, |
298 | .interrupt = ps2mult_interrupt, |
299 | .connect = ps2mult_connect, |
300 | .disconnect = ps2mult_disconnect, |
301 | .reconnect = ps2mult_reconnect, |
302 | }; |
303 | |
304 | module_serio_driver(ps2mult_drv); |
305 | |