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 80 guard(spinlock_irqsave)(&psm->lock); 81 82 if (psm->out_port != port) 83 ps2mult_select_port(psm, port); 84 85 need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls)); 86 87 dev_dbg(&serio->dev, 88 "write: %s%02x\n", need_escape ? "ESC " : "", data); 89 90 if (need_escape) 91 serio_write(mx_port, PS2MULT_ESCAPE); 92 93 serio_write(mx_port, data); 94 95 return 0; 96 } 97 98 static int ps2mult_serio_start(struct serio *serio) 99 { 100 struct ps2mult *psm = serio_get_drvdata(serio->parent); 101 struct ps2mult_port *port = serio->port_data; 102 103 guard(spinlock_irqsave)(&psm->lock); 104 105 port->registered = true; 106 107 return 0; 108 } 109 110 static void ps2mult_serio_stop(struct serio *serio) 111 { 112 struct ps2mult *psm = serio_get_drvdata(serio->parent); 113 struct ps2mult_port *port = serio->port_data; 114 115 guard(spinlock_irqsave)(&psm->lock); 116 117 port->registered = false; 118 } 119 120 static int ps2mult_create_port(struct ps2mult *psm, int i) 121 { 122 struct serio *mx_serio = psm->mx_serio; 123 struct serio *serio; 124 125 serio = kzalloc(sizeof(*serio), GFP_KERNEL); 126 if (!serio) 127 return -ENOMEM; 128 129 strscpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name)); 130 snprintf(serio->phys, sizeof(serio->phys), 131 "%s/port%d", mx_serio->phys, i); 132 serio->id.type = SERIO_8042; 133 serio->write = ps2mult_serio_write; 134 serio->start = ps2mult_serio_start; 135 serio->stop = ps2mult_serio_stop; 136 serio->parent = psm->mx_serio; 137 serio->port_data = &psm->ports[i]; 138 139 psm->ports[i].serio = serio; 140 141 return 0; 142 } 143 144 static void ps2mult_reset(struct ps2mult *psm) 145 { 146 guard(spinlock_irqsave)(&psm->lock); 147 148 serio_write(psm->mx_serio, PS2MULT_SESSION_END); 149 serio_write(psm->mx_serio, PS2MULT_SESSION_START); 150 151 ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]); 152 } 153 154 static int ps2mult_connect(struct serio *serio, struct serio_driver *drv) 155 { 156 struct ps2mult *psm; 157 int i; 158 int error; 159 160 if (!serio->write) 161 return -EINVAL; 162 163 psm = kzalloc(sizeof(*psm), GFP_KERNEL); 164 if (!psm) 165 return -ENOMEM; 166 167 spin_lock_init(&psm->lock); 168 psm->mx_serio = serio; 169 170 for (i = 0; i < PS2MULT_NUM_PORTS; i++) { 171 psm->ports[i].sel = ps2mult_controls[i]; 172 error = ps2mult_create_port(psm, i); 173 if (error) 174 goto err_out; 175 } 176 177 psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT]; 178 179 serio_set_drvdata(serio, psm); 180 error = serio_open(serio, drv); 181 if (error) 182 goto err_out; 183 184 ps2mult_reset(psm); 185 186 for (i = 0; i < PS2MULT_NUM_PORTS; i++) { 187 struct serio *s = psm->ports[i].serio; 188 189 dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys); 190 serio_register_port(s); 191 } 192 193 return 0; 194 195 err_out: 196 while (--i >= 0) 197 kfree(psm->ports[i].serio); 198 kfree(psm); 199 return error; 200 } 201 202 static void ps2mult_disconnect(struct serio *serio) 203 { 204 struct ps2mult *psm = serio_get_drvdata(serio); 205 206 /* Note that serio core already take care of children ports */ 207 serio_write(serio, PS2MULT_SESSION_END); 208 serio_close(serio); 209 kfree(psm); 210 211 serio_set_drvdata(serio, NULL); 212 } 213 214 static int ps2mult_reconnect(struct serio *serio) 215 { 216 struct ps2mult *psm = serio_get_drvdata(serio); 217 218 ps2mult_reset(psm); 219 220 return 0; 221 } 222 223 static irqreturn_t ps2mult_interrupt(struct serio *serio, 224 unsigned char data, unsigned int dfl) 225 { 226 struct ps2mult *psm = serio_get_drvdata(serio); 227 struct ps2mult_port *in_port; 228 229 dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl); 230 231 guard(spinlock_irqsave)(&psm->lock); 232 233 if (psm->escape) { 234 psm->escape = false; 235 in_port = psm->in_port; 236 if (in_port->registered) 237 serio_interrupt(in_port->serio, data, dfl); 238 goto out; 239 } 240 241 switch (data) { 242 case PS2MULT_ESCAPE: 243 dev_dbg(&serio->dev, "ESCAPE\n"); 244 psm->escape = true; 245 break; 246 247 case PS2MULT_BSYNC: 248 dev_dbg(&serio->dev, "BSYNC\n"); 249 psm->in_port = psm->out_port; 250 break; 251 252 case PS2MULT_SESSION_START: 253 dev_dbg(&serio->dev, "SS\n"); 254 break; 255 256 case PS2MULT_SESSION_END: 257 dev_dbg(&serio->dev, "SE\n"); 258 break; 259 260 case PS2MULT_KB_SELECTOR: 261 dev_dbg(&serio->dev, "KB\n"); 262 psm->in_port = &psm->ports[PS2MULT_KBD_PORT]; 263 break; 264 265 case PS2MULT_MS_SELECTOR: 266 dev_dbg(&serio->dev, "MS\n"); 267 psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT]; 268 break; 269 270 default: 271 in_port = psm->in_port; 272 if (in_port->registered) 273 serio_interrupt(in_port->serio, data, dfl); 274 break; 275 } 276 277 out: 278 return IRQ_HANDLED; 279 } 280 281 static struct serio_driver ps2mult_drv = { 282 .driver = { 283 .name = "ps2mult", 284 }, 285 .description = "TQC PS/2 Multiplexer driver", 286 .id_table = ps2mult_serio_ids, 287 .interrupt = ps2mult_interrupt, 288 .connect = ps2mult_connect, 289 .disconnect = ps2mult_disconnect, 290 .reconnect = ps2mult_reconnect, 291 }; 292 293 module_serio_driver(ps2mult_drv); 294