xkbd-0.8.15_bthid/src/hidcd.c

   1 /*
   2  *  xkbd-bthid
   3  *
   4  *  Collin R. Mulliner <collin@betaversion.net>
   5  *
   6  *  http://www.mulliner.org/bluetooth/
   7  *
   8  *  License: GPL
   9  *
  10  */
  11
  12 #include <stdio.h>
  13 #include <errno.h>
  14 #include <fcntl.h>
  15 #include <unistd.h>
  16 #include <stdlib.h>
  17 #include <malloc.h>
  18 #include <syslog.h>
  19 #include <signal.h>
  20 #include <getopt.h>
  21 #include <sys/poll.h>
  22 #include <sys/ioctl.h>
  23 #include <sys/socket.h>
  24
  25 #include <linux/input.h>
  26
  27 #include <bluetooth/bluetooth.h>
  28 #include <bluetooth/hci.h>
  29 #include <bluetooth/hci_lib.h>
  30 #include <bluetooth/l2cap.h>
  31 #include <bluetooth/sdp.h>
  32 #include <bluetooth/hidp.h>
  33
  34
  35
  36 #include "hid.h"
  37
  38 #include "structs.h"
  39 #include "kb.h"
  40 #include "button.h"
  41
  42 // from xkbd.c
  43 extern int bthid_pid;
  44
  45 //int cs;
  46 //int is;
  47 int es[2];
  48
  49 int bthid_open()
  50 {
  51         pipe(es);
  52 }
  53
  54 int bthid_close()
  55 {
  56         close(es[0]);
  57         close(es[1]);
  58 }
  59
  60 int bthid_send(int key, int updown)
  61 {
  62         struct input_event e;
  63
  64         bzero(&e, sizeof(e));
  65         e.code = key;
  66         e.value = updown;
  67         if (key == 0xffff) e.type = 0xff;
  68         else e.type = EV_KEY;
  69         write(es[1], &e, sizeof(e));
  70         return(1);
  71 }
  72
  73 static int l2cap_listen(const bdaddr_t *bdaddr, unsigned short psm, int lm, int backlog)
  74 {
  75         struct sockaddr_l2 addr;
  76         struct l2cap_options opts;
  77         int sk;
  78
  79         if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
  80                 return -1;
  81
  82         memset(&addr, 0, sizeof(addr));
  83         addr.l2_family = AF_BLUETOOTH;
  84         bacpy(&addr.l2_bdaddr, bdaddr);
  85         addr.l2_psm = htobs(psm);
  86
  87         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
  88                 close(sk);
  89                 return -1;
  90         }
  91
  92         setsockopt(sk, SOL_L2CAP, L2CAP_LM, &lm, sizeof(lm));
  93
  94         memset(&opts, 0, sizeof(opts));
  95         opts.imtu = HIDP_DEFAULT_MTU;
  96         opts.omtu = HIDP_DEFAULT_MTU;
  97         opts.flush_to = 0xffff;
  98
  99         setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts));
 100
 101         if (listen(sk, backlog) < 0) {
 102                 close(sk);
 103                 return -1;
 104         }
 105
 106         return sk;
 107 }
 108
 109 static int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm)
 110 {
 111         struct sockaddr_l2 addr;
 112         struct l2cap_options opts;
 113         int sk;
 114
 115         if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
 116                 return -1;
 117
 118         memset(&addr, 0, sizeof(addr));
 119         addr.l2_family  = AF_BLUETOOTH;
 120         bacpy(&addr.l2_bdaddr, src);
 121
 122         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 123                 close(sk);
 124                 return -1;
 125         }
 126
 127         memset(&opts, 0, sizeof(opts));
 128         opts.imtu = HIDP_DEFAULT_MTU;
 129         opts.omtu = HIDP_DEFAULT_MTU;
 130         opts.flush_to = 0xffff;
 131
 132         setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts));
 133
 134         memset(&addr, 0, sizeof(addr));
 135         addr.l2_family  = AF_BLUETOOTH;
 136         bacpy(&addr.l2_bdaddr, dst);
 137         addr.l2_psm = htobs(psm);
 138
 139         if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 140                 close(sk);
 141                 return -1;
 142         }
 143
 144         return sk;
 145 }
 146
 147 static int l2cap_accept(int sk, bdaddr_t *bdaddr)
 148 {
 149         struct sockaddr_l2 addr;
 150         socklen_t addrlen;
 151         int nsk;
 152
 153         memset(&addr, 0, sizeof(addr));
 154         addrlen = sizeof(addr);
 155
 156         if ((nsk = accept(sk, (struct sockaddr *) &addr, &addrlen)) < 0)
 157                 return -1;
 158
 159         if (bdaddr)
 160                 bacpy(bdaddr, &addr.l2_bdaddr);
 161
 162         return nsk;
 163 }
 164
 165 //int main(int argc, char **argv)
 166 int bthid(int ip)
 167 {
 168         unsigned char pkg[12];
 169         //int ip;
 170         int i = 0;
 171         struct input_event ie;
 172         bdaddr_t dst;
 173         struct pollfd pf[3];
 174         unsigned char ib[1024];
 175         unsigned char cb[1024];
 176         int state = 0;
 177         unsigned char modifiers = 0;
 178         unsigned char modifiers_old = 0;
 179         int press = 0;
 180         int press_old = 0;
 181         int bitmask[8] = {0};
 182         int is, iss, cs, css;
 183
 184
 185         css = l2cap_listen(BDADDR_ANY, L2CAP_PSM_HIDP_CTRL, 0, 1);
 186         perror("");
 187         iss = l2cap_listen(BDADDR_ANY, L2CAP_PSM_HIDP_INTR, 0, 1);
 188         perror("");
 189
 190         for (;;) {
 191
 192         cs = l2cap_accept(css, NULL);
 193         //cs = l2cap_connect(BDADDR_ANY, &dst, L2CAP_PSM_HIDP_CTRL);
 194         //perror("");
 195         is = l2cap_accept(iss, NULL);
 196         //is = l2cap_connect(BDADDR_ANY, &dst, L2CAP_PSM_HIDP_INTR);
 197         //perror("");
 198
 199         pf[0].fd = ip;
 200         pf[0].events = POLLIN | POLLERR | POLLHUP;
 201         pf[1].fd = cs;
 202         pf[1].events = POLLIN | POLLERR | POLLHUP;
 203         pf[2].fd = is;
 204         pf[2].events = POLLIN | POLLERR | POLLHUP;
 205
 206         while (1) {
 207                 pf[0].revents = 0;
 208                 pf[1].revents = 0;
 209                 pf[2].revents = 0;
 210
 211                 if (poll(pf, 3, -1) <= 0) {
 212                         goto out;
 213                 }
 214
 215                 bzero(pkg, 12);
 216
 217                 if (pf[0].revents) {
 218                         if (read(ip, &ie, sizeof(ie)) <= 0) {
 219                                 perror("read ip");
 220                                 exit(-1);
 221                         }
 222                         if (ie.type == 0xff) {
 223                                 // quit key
 224                                 if (ie.value == 0xffff) {
 225                                         //close(is);
 226                                         //close(cs);
 227                                         //close(ip);
 228                                         //exit(0);
 229                                         goto out;
 230                                 }
 231                         }
 232                         else if (ie.type == 0x01) {
 233                                 if (state == 1) {
 234
 235                                         modifiers_old = modifiers;
 236                                         press_old = press;
 237
 238                                         switch (ie.code) {
 239                                         case KEY_LEFTSHIFT:
 240                                                 bitmask[1] = (ie.value ? 1 : 0);
 241                                                 break;
 242                                         case KEY_RIGHTSHIFT:
 243                                                 bitmask[5] = (ie.value ? 1 : 0);
 244                                                 break;
 245                                         case KEY_LEFTALT:
 246                                                 bitmask[2] = (ie.value ? 1 : 0);
 247                                                 break;
 248                                         case KEY_RIGHTALT:
 249                                                 bitmask[6] = (ie.value ? 1 : 0);
 250                                                 break;
 251                                         case KEY_LEFTCTRL:
 252                                                 bitmask[0] = (ie.value ? 1 : 0);
 253                                                 break;
 254                                         case KEY_RIGHTCTRL:
 255                                                 bitmask[4] = (ie.value ? 1 : 0);
 256                                                 break;
 257                                         default:
 258                                                 if (ie.value > 0) {
 259                                                         pkg[4] = keycode2hidp[ie.code];
 260                                                         //printf("keycode=%d, hidp=%d\n",ie.code,pkg[4]);
 261                                                         press++;
 262                                                 }
 263                                                 else {
 264                                                         press--;
 265                                                 }
 266                                                 break;
 267                                         }
 268
 269                                         modifiers = 0;
 270                                         for (i = 0; i < 8; i++) {
 271                                                 modifiers |= (bitmask[i] << i);
 272                                         }
 273                                         //fprintf(stderr, "modifiers: 0x%02x\n", modifiers);
 274
 275                                         if (press != press_old || modifiers != modifiers_old) {
 276                                                 pkg[0] = 0xa1;
 277                                                 pkg[1] = 0x01;
 278                                                 pkg[2] = modifiers;
 279                                                 pkg[3] = 0x00;
 280                                                 //pkg[4] = 0x00; // the key code
 281                                                 pkg[5] = 0x00;
 282                                                 pkg[6] = 0x00;
 283                                                 pkg[7] = 0x00;
 284                                                 pkg[8] = 0x00;
 285                                                 pkg[9] = 0x00;
 286
 287                                         if (write(is, pkg, 10) <= 0) {
 288                                                         perror("write");
 289                                                         exit(-1);
 290                                                 }
 291                                         }
 292                                 }
 293                         }
 294                 }
 295                 if (pf[1].revents) {
 296                         int size;
 297                         int i;
 298                         if ((size = read(cs, cb, sizeof(cb))) <= 0) {
 299                                 perror("read cs");
 300                                 //exit(-1);
 301                                 goto out;
 302                         }
 303                         //printf("cs(%d)\n", size);
 304
 305 /*                      for (i = 0; i < size; i++)
 306                                 printf("%02x",cb[i]);
 307                         printf("\n");
 308 */
 309                         if (state == 0 && size == 1 && cb[0] == 0x70) {
 310                                 //printf("got set_protocol BOOT\n");
 311                                 pkg[0] = 0;
 312                                 write(cs, pkg, 1);
 313                                 state = 1;
 314                         }
 315                 }
 316                 if (pf[2].revents) {
 317                         int size;
 318                         int i;
 319                         if ((size = read(is, ib, sizeof(ib))) <= 0) {
 320                                 perror("read is");
 321                                 //exit(-1);
 322                                 goto out;
 323                         }
 324 /*                      printf("is(%d): ", size);
 325                         for (i = 0; i < size; i++)
 326                                 printf("%02x",ib[i]);
 327                         printf("\n");
 328 */
 329                 }
 330         }
 331 out:
 332         printf("disconnected\n");
 333         close(cs);
 334         close(is);
 335         }
 336 }
 337
 338 int bthid_process_keypress(button *active_but, int updown)
 339 {
 340         int new_state = active_but->kb->state;
 341
 342
 343         if (active_but->modifier/* is a shift / ctrl / mod pressed */
 344                         && !(active_but->modifier & BUT_CAPS) ) {
 345                 if (active_but->kb->state_locked & active_but->modifier) {
 346                         /* was locked then unset & unlock */
 347                         active_but->kb->state_locked ^= active_but->modifier;
 348                         new_state ^= active_but->modifier;
 349                 }
 350                 else if (new_state & active_but->modifier) {
 351                         /* was set then lock */
 352                         active_but->kb->state_locked ^= active_but->modifier;
 353                 }
 354                 else {
 355                         /* was unset then set */
 356                         new_state ^= active_but->modifier;
 357                 }
 358         }
 359         /* deal with caps key - maybe this can go above now ?*/
 360         else if (active_but->modifier & BUT_CAPS) {
 361                 new_state ^= KB_STATE_CAPS; /* turn caps on/off */
 362         }
 363         else if ((active_but->kb->state & KB_STATE_SHIFT)
 364                         || (active_but->kb->state & KB_STATE_MOD)
 365                         || (active_but->kb->state & KB_STATE_CTRL)
 366                         || (active_but->kb->state & KB_STATE_META)
 367                         || (active_but->kb->state & KB_STATE_ALT) ) {
 368                 /* check if the kbd is already in a state and reset it
 369                  *        leaving caps key state alone */
 370                 new_state &= KB_STATE_CAPS;
 371                 new_state |= active_but->kb->state_locked;
 372         }
 373
 374 /*
 375         if (active_but->kb->state & KB_STATE_SHIFT) {
 376                 bthid_send(42, 1);
 377         }
 378         else if (active_but->kb->state & KB_STATE_CTRL) {
 379                 bthid_send(29, 1);
 380         }
 381         else if (active_but->kb->state & KB_STATE_ALT) {
 382                 bthid_send(56, 1);
 383         }
 384 */
 385
 386         // ALT_L
 387         if (active_but->scancode == 56) {
 388                 if (updown) {
 389                         if (active_but->kb->bthid_state & KB_STATE_ALT_L) {
 390                                 bthid_send(56, 0);
 391                                 active_but->kb->bthid_state &= (!KB_STATE_ALT_L);
 392                         }
 393                         else {
 394                                 bthid_send(56, 1);
 395                                 active_but->kb->bthid_state |= KB_STATE_ALT_L;
 396                         }
 397                 }
 398         }
 399         // CTRL_L
 400         else if (active_but->scancode == 29) {
 401                 if (updown) {
 402                         if (active_but->kb->bthid_state & KB_STATE_CTRL_L) {
 403                                 bthid_send(29, 0);
 404                                 active_but->kb->bthid_state &= (!KB_STATE_CTRL_L);
 405                         }
 406                         else {
 407                                 bthid_send(29, 1);
 408                                 active_but->kb->bthid_state |= KB_STATE_CTRL_L;
 409                         }
 410                 }
 411         }
 412         // SHIFT_L
 413         else if (active_but->scancode == 42) {
 414                 if (updown) {
 415                         if (active_but->kb->bthid_state & KB_STATE_SHIFT_L) {
 416                                 bthid_send(42, 0);
 417                                 active_but->kb->bthid_state &= (!KB_STATE_SHIFT_L);
 418                         }
 419                         else {
 420                                 bthid_send(42, 1);
 421                                 active_but->kb->bthid_state |= KB_STATE_SHIFT_L;
 422                         }
 423                 }
 424         }
 425         // release shift/ctrl/alt when pressing other key
 426         else {
 427                 // first send key
 428                 bthid_send(active_but->scancode, updown);
 429                 if (active_but->scancode == 0xffff) {
 430                         close(es[1]);
 431                         kill(bthid_pid, SIGTERM);
 432                         exit(0);
 433                 }
 434
 435                 if (active_but->kb->bthid_state & KB_STATE_ALT_L) {
 436                         bthid_send(56, 0);
 437                         active_but->kb->bthid_state &= (!KB_STATE_ALT_L);
 438                 }
 439                 if (active_but->kb->bthid_state & KB_STATE_CTRL_L) {
 440                         bthid_send(29, 0);
 441                         active_but->kb->bthid_state &= (!KB_STATE_CTRL_L);
 442                 }
 443                 if (active_but->kb->bthid_state & KB_STATE_SHIFT_L) {
 444                         bthid_send(42, 0);
 445                         active_but->kb->bthid_state &= (!KB_STATE_SHIFT_L);
 446                 }
 447         }
 448
 449         return new_state;
 450 }