0001
0002
0003
0004
0005
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
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
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);