Commit | Line | Data |
---|---|---|
d2912cb1 | 1 | // SPDX-License-Identifier: GPL-2.0-only |
fc58d12b DES |
2 | /* |
3 | * TQC PS/2 Multiplexer driver | |
4 | * | |
5 | * Copyright (C) 2010 Dmitry Eremin-Solenikov | |
fc58d12b DES |
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(mx_serio, 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(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(ps2mult_controls, data, 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(mx_port, PS2MULT_ESCAPE); | |
93 | ||
94 | serio_write(mx_port, data); | |
95 | ||
96 | spin_unlock_irqrestore(&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->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(&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->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(&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(sizeof(struct serio), GFP_KERNEL); | |
131 | if (!serio) | |
132 | return -ENOMEM; | |
133 | ||
134 | strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name)); | |
135 | snprintf(serio->phys, sizeof(serio->phys), | |
136 | "%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(psm->mx_serio, PS2MULT_SESSION_END); | |
156 | serio_write(psm->mx_serio, PS2MULT_SESSION_START); | |
157 | ||
158 | ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]); | |
159 | ||
160 | spin_unlock_irqrestore(&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(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, 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(psm->ports[i].serio); | |
0e86eb29 | 207 | kfree(psm); |
fc58d12b DES |
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(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(in_port->serio, data, 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(in_port->serio, data, dfl); | |
284 | break; | |
285 | } | |
286 | ||
287 | out: | |
288 | spin_unlock_irqrestore(&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 | ||
65ac9f7a | 304 | module_serio_driver(ps2mult_drv); |