drivers/input/serio/ps2mult.c

   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         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
 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);