ps2mult.c 6.6 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304
  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. #include <linux/kernel.h>
  8. #include <linux/slab.h>
  9. #include <linux/module.h>
  10. #include <linux/serio.h>
  11. MODULE_AUTHOR("Dmitry Eremin-Solenikov <dbaryshkov@gmail.com>");
  12. MODULE_DESCRIPTION("TQC PS/2 Multiplexer driver");
  13. MODULE_LICENSE("GPL");
  14. #define PS2MULT_KB_SELECTOR 0xA0
  15. #define PS2MULT_MS_SELECTOR 0xA1
  16. #define PS2MULT_ESCAPE 0x7D
  17. #define PS2MULT_BSYNC 0x7E
  18. #define PS2MULT_SESSION_START 0x55
  19. #define PS2MULT_SESSION_END 0x56
  20. struct ps2mult_port {
  21. struct serio *serio;
  22. unsigned char sel;
  23. bool registered;
  24. };
  25. #define PS2MULT_NUM_PORTS 2
  26. #define PS2MULT_KBD_PORT 0
  27. #define PS2MULT_MOUSE_PORT 1
  28. struct ps2mult {
  29. struct serio *mx_serio;
  30. struct ps2mult_port ports[PS2MULT_NUM_PORTS];
  31. spinlock_t lock;
  32. struct ps2mult_port *in_port;
  33. struct ps2mult_port *out_port;
  34. bool escape;
  35. };
  36. /* First MUST come PS2MULT_NUM_PORTS selectors */
  37. static const unsigned char ps2mult_controls[] = {
  38. PS2MULT_KB_SELECTOR, PS2MULT_MS_SELECTOR,
  39. PS2MULT_ESCAPE, PS2MULT_BSYNC,
  40. PS2MULT_SESSION_START, PS2MULT_SESSION_END,
  41. };
  42. static const struct serio_device_id ps2mult_serio_ids[] = {
  43. {
  44. .type = SERIO_RS232,
  45. .proto = SERIO_PS2MULT,
  46. .id = SERIO_ANY,
  47. .extra = SERIO_ANY,
  48. },
  49. { 0 }
  50. };
  51. MODULE_DEVICE_TABLE(serio, ps2mult_serio_ids);
  52. static void ps2mult_select_port(struct ps2mult *psm, struct ps2mult_port *port)
  53. {
  54. struct serio *mx_serio = psm->mx_serio;
  55. serio_write(mx_serio, port->sel);
  56. psm->out_port = port;
  57. dev_dbg(&mx_serio->dev, "switched to sel %02x\n", port->sel);
  58. }
  59. static int ps2mult_serio_write(struct serio *serio, unsigned char data)
  60. {
  61. struct serio *mx_port = serio->parent;
  62. struct ps2mult *psm = serio_get_drvdata(mx_port);
  63. struct ps2mult_port *port = serio->port_data;
  64. bool need_escape;
  65. unsigned long flags;
  66. spin_lock_irqsave(&psm->lock, flags);
  67. if (psm->out_port != port)
  68. ps2mult_select_port(psm, port);
  69. need_escape = memchr(ps2mult_controls, data, sizeof(ps2mult_controls));
  70. dev_dbg(&serio->dev,
  71. "write: %s%02x\n", need_escape ? "ESC " : "", data);
  72. if (need_escape)
  73. serio_write(mx_port, PS2MULT_ESCAPE);
  74. serio_write(mx_port, data);
  75. spin_unlock_irqrestore(&psm->lock, flags);
  76. return 0;
  77. }
  78. static int ps2mult_serio_start(struct serio *serio)
  79. {
  80. struct ps2mult *psm = serio_get_drvdata(serio->parent);
  81. struct ps2mult_port *port = serio->port_data;
  82. unsigned long flags;
  83. spin_lock_irqsave(&psm->lock, flags);
  84. port->registered = true;
  85. spin_unlock_irqrestore(&psm->lock, flags);
  86. return 0;
  87. }
  88. static void ps2mult_serio_stop(struct serio *serio)
  89. {
  90. struct ps2mult *psm = serio_get_drvdata(serio->parent);
  91. struct ps2mult_port *port = serio->port_data;
  92. unsigned long flags;
  93. spin_lock_irqsave(&psm->lock, flags);
  94. port->registered = false;
  95. spin_unlock_irqrestore(&psm->lock, flags);
  96. }
  97. static int ps2mult_create_port(struct ps2mult *psm, int i)
  98. {
  99. struct serio *mx_serio = psm->mx_serio;
  100. struct serio *serio;
  101. serio = kzalloc(sizeof(struct serio), GFP_KERNEL);
  102. if (!serio)
  103. return -ENOMEM;
  104. strlcpy(serio->name, "TQC PS/2 Multiplexer", sizeof(serio->name));
  105. snprintf(serio->phys, sizeof(serio->phys),
  106. "%s/port%d", mx_serio->phys, i);
  107. serio->id.type = SERIO_8042;
  108. serio->write = ps2mult_serio_write;
  109. serio->start = ps2mult_serio_start;
  110. serio->stop = ps2mult_serio_stop;
  111. serio->parent = psm->mx_serio;
  112. serio->port_data = &psm->ports[i];
  113. psm->ports[i].serio = serio;
  114. return 0;
  115. }
  116. static void ps2mult_reset(struct ps2mult *psm)
  117. {
  118. unsigned long flags;
  119. spin_lock_irqsave(&psm->lock, flags);
  120. serio_write(psm->mx_serio, PS2MULT_SESSION_END);
  121. serio_write(psm->mx_serio, PS2MULT_SESSION_START);
  122. ps2mult_select_port(psm, &psm->ports[PS2MULT_KBD_PORT]);
  123. spin_unlock_irqrestore(&psm->lock, flags);
  124. }
  125. static int ps2mult_connect(struct serio *serio, struct serio_driver *drv)
  126. {
  127. struct ps2mult *psm;
  128. int i;
  129. int error;
  130. if (!serio->write)
  131. return -EINVAL;
  132. psm = kzalloc(sizeof(*psm), GFP_KERNEL);
  133. if (!psm)
  134. return -ENOMEM;
  135. spin_lock_init(&psm->lock);
  136. psm->mx_serio = serio;
  137. for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
  138. psm->ports[i].sel = ps2mult_controls[i];
  139. error = ps2mult_create_port(psm, i);
  140. if (error)
  141. goto err_out;
  142. }
  143. psm->in_port = psm->out_port = &psm->ports[PS2MULT_KBD_PORT];
  144. serio_set_drvdata(serio, psm);
  145. error = serio_open(serio, drv);
  146. if (error)
  147. goto err_out;
  148. ps2mult_reset(psm);
  149. for (i = 0; i < PS2MULT_NUM_PORTS; i++) {
  150. struct serio *s = psm->ports[i].serio;
  151. dev_info(&serio->dev, "%s port at %s\n", s->name, serio->phys);
  152. serio_register_port(s);
  153. }
  154. return 0;
  155. err_out:
  156. while (--i >= 0)
  157. kfree(psm->ports[i].serio);
  158. kfree(psm);
  159. return error;
  160. }
  161. static void ps2mult_disconnect(struct serio *serio)
  162. {
  163. struct ps2mult *psm = serio_get_drvdata(serio);
  164. /* Note that serio core already take care of children ports */
  165. serio_write(serio, PS2MULT_SESSION_END);
  166. serio_close(serio);
  167. kfree(psm);
  168. serio_set_drvdata(serio, NULL);
  169. }
  170. static int ps2mult_reconnect(struct serio *serio)
  171. {
  172. struct ps2mult *psm = serio_get_drvdata(serio);
  173. ps2mult_reset(psm);
  174. return 0;
  175. }
  176. static irqreturn_t ps2mult_interrupt(struct serio *serio,
  177. unsigned char data, unsigned int dfl)
  178. {
  179. struct ps2mult *psm = serio_get_drvdata(serio);
  180. struct ps2mult_port *in_port;
  181. unsigned long flags;
  182. dev_dbg(&serio->dev, "Received %02x flags %02x\n", data, dfl);
  183. spin_lock_irqsave(&psm->lock, flags);
  184. if (psm->escape) {
  185. psm->escape = false;
  186. in_port = psm->in_port;
  187. if (in_port->registered)
  188. serio_interrupt(in_port->serio, data, dfl);
  189. goto out;
  190. }
  191. switch (data) {
  192. case PS2MULT_ESCAPE:
  193. dev_dbg(&serio->dev, "ESCAPE\n");
  194. psm->escape = true;
  195. break;
  196. case PS2MULT_BSYNC:
  197. dev_dbg(&serio->dev, "BSYNC\n");
  198. psm->in_port = psm->out_port;
  199. break;
  200. case PS2MULT_SESSION_START:
  201. dev_dbg(&serio->dev, "SS\n");
  202. break;
  203. case PS2MULT_SESSION_END:
  204. dev_dbg(&serio->dev, "SE\n");
  205. break;
  206. case PS2MULT_KB_SELECTOR:
  207. dev_dbg(&serio->dev, "KB\n");
  208. psm->in_port = &psm->ports[PS2MULT_KBD_PORT];
  209. break;
  210. case PS2MULT_MS_SELECTOR:
  211. dev_dbg(&serio->dev, "MS\n");
  212. psm->in_port = &psm->ports[PS2MULT_MOUSE_PORT];
  213. break;
  214. default:
  215. in_port = psm->in_port;
  216. if (in_port->registered)
  217. serio_interrupt(in_port->serio, data, dfl);
  218. break;
  219. }
  220. out:
  221. spin_unlock_irqrestore(&psm->lock, flags);
  222. return IRQ_HANDLED;
  223. }
  224. static struct serio_driver ps2mult_drv = {
  225. .driver = {
  226. .name = "ps2mult",
  227. },
  228. .description = "TQC PS/2 Multiplexer driver",
  229. .id_table = ps2mult_serio_ids,
  230. .interrupt = ps2mult_interrupt,
  231. .connect = ps2mult_connect,
  232. .disconnect = ps2mult_disconnect,
  233. .reconnect = ps2mult_reconnect,
  234. };
  235. module_serio_driver(ps2mult_drv);