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); 210 kfree(serio); 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 307 static int __init ps2mult_init(void) 308 { 309 return serio_register_driver(&ps2mult_drv); 310 } 311 312 static void __exit ps2mult_exit(void) 313 { 314 serio_unregister_driver(&ps2mult_drv); 315 } 316 317 module_init(ps2mult_init); 318 module_exit(ps2mult_exit); 319