1
2
3
4
5
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
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 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
101static 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
114static 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
125static 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
149static 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
163static 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
204err_out:
205 while (--i >= 0)
206 kfree(psm->ports[i].serio);
207 kfree(psm);
208 return error;
209}
210
211static void ps2mult_disconnect(struct serio *serio)
212{
213 struct ps2mult *psm = serio_get_drvdata(serio);
214
215
216 serio_write(serio, PS2MULT_SESSION_END);
217 serio_close(serio);
218 kfree(psm);
219
220 serio_set_drvdata(serio, NULL);
221}
222
223static 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
232static 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
292static 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
304module_serio_driver(ps2mult_drv);
305
