--- /dev/null
+/*
+ * TQC PS/2 Multiplexer driver
+ *
+ * Copyright (C) 2010 Dmitry Eremin-Solenikov
+ *
+ * This program is free software; you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License version 2 as published by
+ * the Free Software Foundation.
+ */
+
+
+#include <linux/kernel.h>
+#include <linux/slab.h>
+#include <linux/module.h>
+#include <linux/serio.h>
+
+MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
+MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
+MODULE_LICENSE("GPL");
+
+#define PS2MULT_KB_SELECTOR 0xA0
+#define PS2MULT_MS_SELECTOR 0xA1
+#define PS2MULT_ESCAPE 0x7D
+#define PS2MULT_BSYNC 0x7E
+#define PS2MULT_SESSION_START 0x55
+#define PS2MULT_SESSION_END 0x56
+
+struct ps2mult_port {
+ struct serio *serio;
+ unsigned char sel;
+ bool registered;
+};
+
+#define PS2MULT_NUM_PORTS 2
+#define PS2MULT_KBD_PORT 0
+#define PS2MULT_MOUSE_PORT 1
+
+struct ps2mult {
+ struct serio *mx_serio;
+ struct ps2mult_port ports[PS2MULT_NUM_PORTS];
+
+ spinlock_t lock;
+ struct ps2mult_port *in_port;
+ struct ps2mult_port *out_port;
+ bool escape;
+};
+
+/* First MUST come PS2MULT_NUM_PORTS selectors */
+static const unsigned char ps2mult_controls[] = {
+ PS2MULT_KB_SELECTOR, PS2MULT_MS_SELECTOR,
+ PS2MULT_ESCAPE, PS2MULT_BSYNC,
+ PS2MULT_SESSION_START, PS2MULT_SESSION_END,
+};
+
+static const struct serio_device_id ps2mult_serio_ids[] = {
+ {
+ .type = SERIO_RS232,
+ .proto = SERIO_PS2MULT,
+ .id = SERIO_ANY,
+ .extra = SERIO_ANY,
+ },
+ { 0 }
+};
+
+MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);
+
+static void ps2mult_select_port(struct ps2mult *psm, struct ps2mult_port *port)
+{
+ struct serio *mx_serio = psm->mx_serio;
+
+ serio_write(mx_serio, port->sel);
+ psm->out_port = port;
+ dev_dbg(&mx_serio->dev, "switched to sel %02x\n", port->sel);
+}
+
+static int ps2mult_serio_write(struct serio *serio, unsigned char data)
+{
+ struct serio *mx_port = serio->parent;
+ struct ps2mult *psm = serio_get_drvdata(mx_port);
+ struct ps2mult_port *port = serio->port_data;
+ bool need_escape;
+ unsigned long flags;
+
+ spin_lock_irqsave(&psm->lock, flags);
+
+ if (psm->out_port != port)
+ ps2mult_select_port(psm, port);
+
+ need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls));
+
+ dev_dbg(&serio->dev,
+ "write: %s%02x\n", need_escape ? "ESC " : "", data);
+
+ if (need_escape)
+ serio_write(mx_port, PS2MULT_ESCAPE);
+
+ serio_write(mx_port, data);
+
+ spin_unlock_irqrestore(&psm->lock, flags);
+
+ return 0;
+}
+
+static int ps2mult_serio_start(struct serio *serio)
+{
+ struct ps2mult *psm = serio_get_drvdata(serio->parent);
+ struct ps2mult_port *port = serio->port_data;
+ unsigned long flags;
+
+ spin_lock_irqsave(&psm->lock, flags);
+ port->registered = true;
+ spin_unlock_irqrestore(&psm->lock, flags);
+
+ return 0;
+}
+
+static void ps2mult_serio_stop(struct serio *serio)
+{
+ struct ps2mult *psm = serio_get_drvdata(serio->parent);
+ struct ps2mult_port *port = serio->port_data;
+ unsigned long flags;
+
+ spin_lock_irqsave(&psm->lock, flags);
+ port->registered = false;
+ spin_unlock_irqrestore(&psm->lock, flags);
+}
+
+static int ps2mult_create_port(struct ps2mult *psm, int i)
+{
+ struct serio *mx_serio = psm->mx_serio;
+ struct serio *serio;
+
+ serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
+ if (!serio)
+ return -ENOMEM;
+
+ strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name));
+ snprintf(serio->phys, sizeof(serio->phys),
+ "%s/port%d", mx_serio->phys, i);
+ serio->id.type = SERIO_8042;
+ serio->write = ps2mult_serio_write;
+ serio->start = ps2mult_serio_start;
+ serio->stop = ps2mult_serio_stop;
+ serio->parent = psm->mx_serio;
+ serio->port_data = &psm->ports[i];
+
+ psm->ports[i].serio = serio;
+
+ return 0;
+}
+
+static void ps2mult_reset(struct ps2mult *psm)
+{
+ unsigned long flags;
+
+ spin_lock_irqsave(&psm->lock, flags);
+
+ serio_write(psm->mx_serio, PS2MULT_SESSION_END);
+ serio_write(psm->mx_serio, PS2MULT_SESSION_START);
+
+ ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]);
+
+ spin_unlock_irqrestore(&psm->lock, flags);
+}
+
+static int ps2mult_connect(struct serio *serio, struct serio_driver *drv)
+{
+ struct ps2mult *psm;
+ int i;
+ int error;
+
+ if (!serio->write)
+ return -EINVAL;
+
+ psm = kzalloc(sizeof(*psm), GFP_KERNEL);
+ if (!psm)
+ return -ENOMEM;
+
+ spin_lock_init(&psm->lock);
+ psm->mx_serio = serio;
+
+ for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
+ psm->ports[i].sel = ps2mult_controls[i];
+ error = ps2mult_create_port(psm, i);
+ if (error)
+ goto err_out;
+ }
+
+ psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT];
+
+ serio_set_drvdata(serio, psm);
+ error = serio_open(serio, drv);
+ if (error)
+ goto err_out;
+
+ ps2mult_reset(psm);
+
+ for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
+ struct serio *s = psm->ports[i].serio;
+
+ dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys);
+ serio_register_port(s);
+ }
+
+ return 0;
+
+err_out:
+ while (--i >= 0)
+ kfree(psm->ports[i].serio);
+ kfree(serio);
+ return error;
+}
+
+static void ps2mult_disconnect(struct serio *serio)
+{
+ struct ps2mult *psm = serio_get_drvdata(serio);
+
+ /* Note that serio core already take care of children ports */
+ serio_write(serio, PS2MULT_SESSION_END);
+ serio_close(serio);
+ kfree(psm);
+
+ serio_set_drvdata(serio, NULL);
+}
+
+static int ps2mult_reconnect(struct serio *serio)
+{
+ struct ps2mult *psm = serio_get_drvdata(serio);
+
+ ps2mult_reset(psm);
+
+ return 0;
+}
+
+static irqreturn_t ps2mult_interrupt(struct serio *serio,
+ unsigned char data, unsigned int dfl)
+{
+ struct ps2mult *psm = serio_get_drvdata(serio);
+ struct ps2mult_port *in_port;
+ unsigned long flags;
+
+ dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl);
+
+ spin_lock_irqsave(&psm->lock, flags);
+
+ if (psm->escape) {
+ psm->escape = false;
+ in_port = psm->in_port;
+ if (in_port->registered)
+ serio_interrupt(in_port->serio, data, dfl);
+ goto out;
+ }
+
+ switch (data) {
+ case PS2MULT_ESCAPE:
+ dev_dbg(&serio->dev, "ESCAPE\n");
+ psm->escape = true;
+ break;
+
+ case PS2MULT_BSYNC:
+ dev_dbg(&serio->dev, "BSYNC\n");
+ psm->in_port = psm->out_port;
+ break;
+
+ case PS2MULT_SESSION_START:
+ dev_dbg(&serio->dev, "SS\n");
+ break;
+
+ case PS2MULT_SESSION_END:
+ dev_dbg(&serio->dev, "SE\n");
+ break;
+
+ case PS2MULT_KB_SELECTOR:
+ dev_dbg(&serio->dev, "KB\n");
+ psm->in_port = &psm->ports[PS2MULT_KBD_PORT];
+ break;
+
+ case PS2MULT_MS_SELECTOR:
+ dev_dbg(&serio->dev, "MS\n");
+ psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT];
+ break;
+
+ default:
+ in_port = psm->in_port;
+ if (in_port->registered)
+ serio_interrupt(in_port->serio, data, dfl);
+ break;
+ }
+
+ out:
+ spin_unlock_irqrestore(&psm->lock, flags);
+ return IRQ_HANDLED;
+}
+
+static struct serio_driver ps2mult_drv = {
+ .driver = {
+ .name = "ps2mult",
+ },
+ .description = "TQC PS/2 Multiplexer driver",
+ .id_table = ps2mult_serio_ids,
+ .interrupt = ps2mult_interrupt,
+ .connect = ps2mult_connect,
+ .disconnect = ps2mult_disconnect,
+ .reconnect = ps2mult_reconnect,
+};
+
+static int __init ps2mult_init(void)
+{
+ return serio_register_driver(&ps2mult_drv);
+}
+
+static void __exit ps2mult_exit(void)
+{
+ serio_unregister_driver(&ps2mult_drv);
+}
+
+module_init(ps2mult_init);
+module_exit(ps2mult_exit);
projects / mv-sheeva.git / commitdiff