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