Back to home page

OSCL-LXR

 
 

    


0001 // SPDX-License-Identifier: GPL-2.0-only
0002 /*
0003  * TQC PS/2 Multiplexer driver
0004  *
0005  * Copyright (C) 2010 Dmitry Eremin-Solenikov
0006  */
0007 
0008 
0009 #include <linux/kernel.h>
0010 #include <linux/slab.h>
0011 #include <linux/module.h>
0012 #include <linux/serio.h>
0013 
0014 MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
0015 MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
0016 MODULE_LICENSE("GPL");
0017 
0018 #define PS2MULT_KB_SELECTOR     0xA0
0019 #define PS2MULT_MS_SELECTOR     0xA1
0020 #define PS2MULT_ESCAPE          0x7D
0021 #define PS2MULT_BSYNC           0x7E
0022 #define PS2MULT_SESSION_START       0x55
0023 #define PS2MULT_SESSION_END     0x56
0024 
0025 struct ps2mult_port {
0026     struct serio *serio;
0027     unsigned char sel;
0028     bool registered;
0029 };
0030 
0031 #define PS2MULT_NUM_PORTS   2
0032 #define PS2MULT_KBD_PORT    0
0033 #define PS2MULT_MOUSE_PORT  1
0034 
0035 struct ps2mult {
0036     struct serio *mx_serio;
0037     struct ps2mult_port ports[PS2MULT_NUM_PORTS];
0038 
0039     spinlock_t lock;
0040     struct ps2mult_port *in_port;
0041     struct ps2mult_port *out_port;
0042     bool escape;
0043 };
0044 
0045 /* First MUST come PS2MULT_NUM_PORTS selectors */
0046 static const unsigned char ps2mult_controls[] = {
0047     PS2MULT_KB_SELECTOR, PS2MULT_MS_SELECTOR,
0048     PS2MULT_ESCAPE, PS2MULT_BSYNC,
0049     PS2MULT_SESSION_START, PS2MULT_SESSION_END,
0050 };
0051 
0052 static const struct serio_device_id ps2mult_serio_ids[] = {
0053     {
0054         .type   = SERIO_RS232,
0055         .proto  = SERIO_PS2MULT,
0056         .id = SERIO_ANY,
0057         .extra  = SERIO_ANY,
0058     },
0059     { 0 }
0060 };
0061 
0062 MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);
0063 
0064 static void ps2mult_select_port(struct ps2mult *psm, struct ps2mult_port *port)
0065 {
0066     struct serio *mx_serio = psm->mx_serio;
0067 
0068     serio_write(mx_serio, port->sel);
0069     psm->out_port = port;
0070     dev_dbg(&mx_serio->dev, "switched to sel %02x\n", port->sel);
0071 }
0072 
0073 static int ps2mult_serio_write(struct serio *serio, unsigned char data)
0074 {
0075     struct serio *mx_port = serio->parent;
0076     struct ps2mult *psm = serio_get_drvdata(mx_port);
0077     struct ps2mult_port *port = serio->port_data;
0078     bool need_escape;
0079     unsigned long flags;
0080 
0081     spin_lock_irqsave(&psm->lock, flags);
0082 
0083     if (psm->out_port != port)
0084         ps2mult_select_port(psm, port);
0085 
0086     need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls));
0087 
0088     dev_dbg(&serio->dev,
0089         "write: %s%02x\n", need_escape ? "ESC " : "", data);
0090 
0091     if (need_escape)
0092         serio_write(mx_port, PS2MULT_ESCAPE);
0093 
0094     serio_write(mx_port, data);
0095 
0096     spin_unlock_irqrestore(&psm->lock, flags);
0097 
0098     return 0;
0099 }
0100 
0101 static int ps2mult_serio_start(struct serio *serio)
0102 {
0103     struct ps2mult *psm = serio_get_drvdata(serio->parent);
0104     struct ps2mult_port *port = serio->port_data;
0105     unsigned long flags;
0106 
0107     spin_lock_irqsave(&psm->lock, flags);
0108     port->registered = true;
0109     spin_unlock_irqrestore(&psm->lock, flags);
0110 
0111     return 0;
0112 }
0113 
0114 static void ps2mult_serio_stop(struct serio *serio)
0115 {
0116     struct ps2mult *psm = serio_get_drvdata(serio->parent);
0117     struct ps2mult_port *port = serio->port_data;
0118     unsigned long flags;
0119 
0120     spin_lock_irqsave(&psm->lock, flags);
0121     port->registered = false;
0122     spin_unlock_irqrestore(&psm->lock, flags);
0123 }
0124 
0125 static int ps2mult_create_port(struct ps2mult *psm, int i)
0126 {
0127     struct serio *mx_serio = psm->mx_serio;
0128     struct serio *serio;
0129 
0130     serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
0131     if (!serio)
0132         return -ENOMEM;
0133 
0134     strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name));
0135     snprintf(serio->phys, sizeof(serio->phys),
0136          "%s/port%d", mx_serio->phys, i);
0137     serio->id.type = SERIO_8042;
0138     serio->write = ps2mult_serio_write;
0139     serio->start = ps2mult_serio_start;
0140     serio->stop = ps2mult_serio_stop;
0141     serio->parent = psm->mx_serio;
0142     serio->port_data = &psm->ports[i];
0143 
0144     psm->ports[i].serio = serio;
0145 
0146     return 0;
0147 }
0148 
0149 static void ps2mult_reset(struct ps2mult *psm)
0150 {
0151     unsigned long flags;
0152 
0153     spin_lock_irqsave(&psm->lock, flags);
0154 
0155     serio_write(psm->mx_serio, PS2MULT_SESSION_END);
0156     serio_write(psm->mx_serio, PS2MULT_SESSION_START);
0157 
0158     ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]);
0159 
0160     spin_unlock_irqrestore(&psm->lock, flags);
0161 }
0162 
0163 static int ps2mult_connect(struct serio *serio, struct serio_driver *drv)
0164 {
0165     struct ps2mult *psm;
0166     int i;
0167     int error;
0168 
0169     if (!serio->write)
0170         return -EINVAL;
0171 
0172     psm = kzalloc(sizeof(*psm), GFP_KERNEL);
0173     if (!psm)
0174         return -ENOMEM;
0175 
0176     spin_lock_init(&psm->lock);
0177     psm->mx_serio = serio;
0178 
0179     for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
0180         psm->ports[i].sel = ps2mult_controls[i];
0181         error = ps2mult_create_port(psm, i);
0182         if (error)
0183             goto err_out;
0184     }
0185 
0186     psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT];
0187 
0188     serio_set_drvdata(serio, psm);
0189     error = serio_open(serio, drv);
0190     if (error)
0191         goto err_out;
0192 
0193     ps2mult_reset(psm);
0194 
0195     for (i = 0; i <  PS2MULT_NUM_PORTS; i++) {
0196         struct serio *s = psm->ports[i].serio;
0197 
0198         dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys);
0199         serio_register_port(s);
0200     }
0201 
0202     return 0;
0203 
0204 err_out:
0205     while (--i >= 0)
0206         kfree(psm->ports[i].serio);
0207     kfree(psm);
0208     return error;
0209 }
0210 
0211 static void ps2mult_disconnect(struct serio *serio)
0212 {
0213     struct ps2mult *psm = serio_get_drvdata(serio);
0214 
0215     /* Note that serio core already take care of children ports */
0216     serio_write(serio, PS2MULT_SESSION_END);
0217     serio_close(serio);
0218     kfree(psm);
0219 
0220     serio_set_drvdata(serio, NULL);
0221 }
0222 
0223 static int ps2mult_reconnect(struct serio *serio)
0224 {
0225     struct ps2mult *psm = serio_get_drvdata(serio);
0226 
0227     ps2mult_reset(psm);
0228 
0229     return 0;
0230 }
0231 
0232 static irqreturn_t ps2mult_interrupt(struct serio *serio,
0233                      unsigned char data, unsigned int dfl)
0234 {
0235     struct ps2mult *psm = serio_get_drvdata(serio);
0236     struct ps2mult_port *in_port;
0237     unsigned long flags;
0238 
0239     dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl);
0240 
0241     spin_lock_irqsave(&psm->lock, flags);
0242 
0243     if (psm->escape) {
0244         psm->escape = false;
0245         in_port = psm->in_port;
0246         if (in_port->registered)
0247             serio_interrupt(in_port->serio, data, dfl);
0248         goto out;
0249     }
0250 
0251     switch (data) {
0252     case PS2MULT_ESCAPE:
0253         dev_dbg(&serio->dev, "ESCAPE\n");
0254         psm->escape = true;
0255         break;
0256 
0257     case PS2MULT_BSYNC:
0258         dev_dbg(&serio->dev, "BSYNC\n");
0259         psm->in_port = psm->out_port;
0260         break;
0261 
0262     case PS2MULT_SESSION_START:
0263         dev_dbg(&serio->dev, "SS\n");
0264         break;
0265 
0266     case PS2MULT_SESSION_END:
0267         dev_dbg(&serio->dev, "SE\n");
0268         break;
0269 
0270     case PS2MULT_KB_SELECTOR:
0271         dev_dbg(&serio->dev, "KB\n");
0272         psm->in_port = &psm->ports[PS2MULT_KBD_PORT];
0273         break;
0274 
0275     case PS2MULT_MS_SELECTOR:
0276         dev_dbg(&serio->dev, "MS\n");
0277         psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT];
0278         break;
0279 
0280     default:
0281         in_port = psm->in_port;
0282         if (in_port->registered)
0283             serio_interrupt(in_port->serio, data, dfl);
0284         break;
0285     }
0286 
0287  out:
0288     spin_unlock_irqrestore(&psm->lock, flags);
0289     return IRQ_HANDLED;
0290 }
0291 
0292 static struct serio_driver ps2mult_drv = {
0293     .driver     = {
0294         .name   = "ps2mult",
0295     },
0296     .description    = "TQC PS/2 Multiplexer driver",
0297     .id_table   = ps2mult_serio_ids,
0298     .interrupt  = ps2mult_interrupt,
0299     .connect    = ps2mult_connect,
0300     .disconnect = ps2mult_disconnect,
0301     .reconnect  = ps2mult_reconnect,
0302 };
0303 
0304 module_serio_driver(ps2mult_drv);