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(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 strscpy(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); 207 kfree(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(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 304 module_serio_driver(ps2mult_drv); 305