mousecmd.c 3.5 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177
  1. /* mouse command: poll PS/2 aux (mouse) for 10s, print packets (relative dx/dy + accumulators). */
  2. #include "io.h"
  3. #include "timer.h"
  4. #define PS2_DATA 0x60
  5. #define PS2_STATUS 0x64
  6. #define PS2_CMD 0x64
  7. #define STATUS_OUT_FULL 0x01
  8. #define STATUS_IN_FULL 0x02
  9. #define STATUS_AUX_DATA 0x20
  10. #define MOUSE_ACK 0xFA
  11. #define MOUSE_RESEND 0xFE
  12. static inline unsigned char inb(unsigned short port)
  13. {
  14. unsigned char v;
  15. __asm__ volatile ("inb %1, %0" : "=a"(v) : "Nd"(port));
  16. return v;
  17. }
  18. static inline void outb(unsigned short port, unsigned char v)
  19. {
  20. __asm__ volatile ("outb %0, %1" : : "a"(v), "Nd"(port));
  21. }
  22. static void ps2_wait_input_empty(void)
  23. {
  24. unsigned spin = 100000;
  25. while (spin-- && (inb(PS2_STATUS) & STATUS_IN_FULL))
  26. ;
  27. }
  28. static void mouse_cmd_write(unsigned char val)
  29. {
  30. ps2_wait_input_empty();
  31. outb(PS2_CMD, 0xD4);
  32. ps2_wait_input_empty();
  33. outb(PS2_DATA, val);
  34. }
  35. /* Read next byte from aux port (trackpoint/touchpad), with timeout in timer ticks (~ms). */
  36. static int mouse_read_aux(unsigned char *out, unsigned int timeout_ticks)
  37. {
  38. unsigned int t0 = get_ticks_ms();
  39. while (get_ticks_ms() - t0 < timeout_ticks) {
  40. unsigned char st = inb(PS2_STATUS);
  41. if ((st & (STATUS_OUT_FULL | STATUS_AUX_DATA)) ==
  42. (STATUS_OUT_FULL | STATUS_AUX_DATA)) {
  43. *out = inb(PS2_DATA);
  44. return 0;
  45. }
  46. }
  47. return -1;
  48. }
  49. static int mouse_expect_ack(void)
  50. {
  51. unsigned char b;
  52. if (mouse_read_aux(&b, 500) != 0)
  53. return -1;
  54. if (b == MOUSE_RESEND) {
  55. /* one retry path left to caller */
  56. return -2;
  57. }
  58. if (b != MOUSE_ACK)
  59. return -1;
  60. return 0;
  61. }
  62. static void enable_aux_port(void)
  63. {
  64. ps2_wait_input_empty();
  65. outb(PS2_CMD, 0xA8);
  66. }
  67. static int mouse_enable_streaming(void)
  68. {
  69. int r;
  70. enable_aux_port();
  71. mouse_cmd_write(0xF4);
  72. r = mouse_expect_ack();
  73. if (r == -2) {
  74. mouse_cmd_write(0xF4);
  75. r = mouse_expect_ack();
  76. }
  77. return r;
  78. }
  79. static void mouse_disable_streaming(void)
  80. {
  81. mouse_cmd_write(0xF5);
  82. (void)mouse_expect_ack();
  83. }
  84. static void put_uint(unsigned int n)
  85. {
  86. char buf[12];
  87. int i = 10;
  88. buf[11] = '\0';
  89. if (n == 0) {
  90. io_putchar('0');
  91. return;
  92. }
  93. while (n && i >= 0) {
  94. buf[i--] = (char)('0' + (n % 10u));
  95. n /= 10u;
  96. }
  97. io_putstr(buf + i + 1);
  98. }
  99. static void put_sdec(int v)
  100. {
  101. if (v < 0) {
  102. io_putchar('-');
  103. put_uint(0u - (unsigned int)v);
  104. } else
  105. put_uint((unsigned int)v);
  106. }
  107. void program_mouse_main(void)
  108. {
  109. enum { RUN_MS = 10000 };
  110. unsigned int t_start;
  111. int acc_x = 0, acc_y = 0;
  112. unsigned char pkt[3];
  113. unsigned int pkt_idx = 0;
  114. io_putstr("PS/2 mouse: 10s (aux poll, 3-byte packets)\n");
  115. if (mouse_enable_streaming() != 0) {
  116. io_putstr("mouse: enable failed (no ACK?)\n");
  117. return;
  118. }
  119. t_start = get_ticks_ms();
  120. while (get_ticks_ms() - t_start < RUN_MS) {
  121. unsigned char st = inb(PS2_STATUS);
  122. if ((st & (STATUS_OUT_FULL | STATUS_AUX_DATA)) ==
  123. (STATUS_OUT_FULL | STATUS_AUX_DATA)) {
  124. unsigned char b = inb(PS2_DATA);
  125. pkt[pkt_idx++] = b;
  126. if (pkt_idx == 3) {
  127. unsigned char b0 = pkt[0];
  128. int dx = (int)(signed char)pkt[1];
  129. int dy = (int)(signed char)pkt[2];
  130. if (b0 & 0x10)
  131. dx |= ~0xFF;
  132. if (b0 & 0x20)
  133. dy |= ~0xFF;
  134. /* overflow bits in b0: do not add to accumulators */
  135. if (!(b0 & 0xC0)) {
  136. acc_x += dx;
  137. acc_y += dy;
  138. }
  139. put_uint(get_ticks_ms() - t_start);
  140. io_putchar(' ');
  141. put_uint(b0 & 7);
  142. io_putchar(' ');
  143. put_sdec(dx);
  144. io_putchar(' ');
  145. put_sdec(dy);
  146. io_putchar(' ');
  147. put_sdec(acc_x);
  148. io_putchar(' ');
  149. put_sdec(acc_y);
  150. io_putchar('\n');
  151. pkt_idx = 0;
  152. }
  153. }
  154. }
  155. mouse_disable_streaming();
  156. io_putstr("PS/2 mouse: done\n");
  157. }