Commit | Line | Data |
---|---|---|
fc58d12b DES |
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 | ||
17 | MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>"); | |
18 | MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver"); | |
19 | MODULE_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 | ||
28 | struct 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 | ||
38 | struct 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 */ | |
49 | static 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 | ||
55 | static 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 | ||
65 | MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids); | |
66 | ||
67 | static 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 | ||
76 | static 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 | ||
104 | static 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 | ||
117 | static 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 | ||
128 | static 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 | ||
152 | static 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 | ||
166 | static 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 | ||
207 | err_out: | |
208 | while (--i >= 0) | |
209 | kfree(psm->ports[i].serio); | |
0e86eb29 | 210 | kfree(psm); |
fc58d12b DES |
211 | return error; |
212 | } | |
213 | ||
214 | static 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 | ||
226 | static 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 | ||
235 | static 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 | ||
295 | static 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 | ||
65ac9f7a | 307 | module_serio_driver(ps2mult_drv); |