drivers/input/serio/ps2mult.c

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