wii_us.c

   1 /*
   2  * Wiimote Bluetooth mouse driver
   3  *
   4  * Userspace Part
   5  *
   6  * Copyright (C) 2009, 2010  Daniel Borkmann <daniel@netsniff-ng.org>
   7  *
   8  * This program is free software; you can redistribute it and/or modify
   9  * it under the terms of the GNU General Public License as published by
  10  * the Free Software Foundation; either version 2 of the License, or
  11  * (at your option) any later version.
  12  *
  13  * This program is distributed in the hope that it will be useful,
  14  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  15  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16  * GNU General Public License for more details.
  17  *
  18  * You should have received a copy of the GNU General Public License
  19  * along with this program; if not, write to the Free Software
  20  * Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  21  */
  22
  23 #define PROGNAME_STRING  "wiimote"
  24 #define VERSION_STRING   "0.5.0"
  25
  26 #include <stdio.h>
  27 #include <stdlib.h>
  28 #include <stdint.h>
  29 #include <unistd.h>
  30 #include <fcntl.h>
  31 #include <math.h>
  32 #include <signal.h>
  33 #include <getopt.h>
  34 #include <ctype.h>
  35 #include <assert.h>
  36 #include <errno.h>
  37 #include <pthread.h>
  38
  39 #include <sys/socket.h>
  40
  41 #include <bluetooth/bluetooth.h>
  42 #include <bluetooth/l2cap.h>
  43 #include <bluetooth/hci.h>
  44 #include <bluetooth/hci_lib.h>
  45 #include <bluetooth/sdp.h>
  46 #include <bluetooth/hidp.h>
  47
  48 #include "wii.h"
  49
  50 static volatile sig_atomic_t sigint = 0;
  51
  52 struct wiimote_ctl {
  53         char *address;
  54         char *device;
  55         int sock_ctrl;
  56         int sock_intr;
  57         int fd_device;
  58 };
  59
  60 static struct option long_options[] = {
  61         {"addr",    required_argument, 0, 'a'},
  62         {"dev",     required_argument, 0, 'd'},
  63         {"scan",    no_argument,       0, 's'},
  64         {"version", no_argument,       0, 'v'},
  65         {"help",    no_argument,       0, 'h'},
  66         {0, 0, 0, 0}
  67 };
  68
  69 /*
  70  *  taken from BlueZ
  71  */
  72 static int l2cap_connect(bdaddr_t *src, bdaddr_t *dst, unsigned short psm)
  73 {
  74         struct sockaddr_l2 addr;
  75         struct l2cap_options opts;
  76         int sk;
  77
  78         if ((sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP)) < 0)
  79                 return -1;
  80
  81         memset(&addr, 0, sizeof(addr));
  82         addr.l2_family  = AF_BLUETOOTH;
  83         bacpy(&addr.l2_bdaddr, src);
  84
  85         if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
  86                 close(sk);
  87                 return -1;
  88         }
  89
  90         memset(&opts, 0, sizeof(opts));
  91         opts.imtu = HIDP_DEFAULT_MTU;
  92         opts.omtu = HIDP_DEFAULT_MTU;
  93         opts.flush_to = 0xffff;
  94
  95         setsockopt(sk, SOL_L2CAP, L2CAP_OPTIONS, &opts, sizeof(opts));
  96
  97         memset(&addr, 0, sizeof(addr));
  98         addr.l2_family  = AF_BLUETOOTH;
  99         bacpy(&addr.l2_bdaddr, dst);
 100         addr.l2_psm = htobs(psm);
 101
 102         if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
 103                 close(sk);
 104                 return -1;
 105         }
 106
 107         return sk;
 108 }
 109
 110 static void version(void)
 111 {
 112         printf("%s %s\n", PROGNAME_STRING, VERSION_STRING);
 113         exit(EXIT_SUCCESS);
 114 }
 115
 116 static void help(void)
 117 {
 118         printf("%s %s\n\n", PROGNAME_STRING, VERSION_STRING);
 119         printf("Usage: wiimote [options]\n\n");
 120         printf("  -a|--addr <arg>   BT address to connect to\n");
 121         printf("  -d|--dev <arg>    Wiimote device file (e.g. /dev/wiimote)\n");
 122         printf("  -s|--scan         Scan for Wiimote devices\n");
 123         printf("  -v|--version      Prints out version\n");
 124         printf("  -h|--help         Prints out this help\n\n");
 125         printf("Please report bugs to <daniel@netsniff-ng.org>\n");
 126         printf("Copyright (C) 2009, 2010 Daniel Borkmann\n");
 127         printf("License: GNU GPL version 2\n");
 128         printf("This is free software: you are free to change and "
 129                "redistribute it.\n");
 130         printf("There is NO WARRANTY, to the extent permitted by law.\n");
 131         exit(EXIT_SUCCESS);
 132 }
 133
 134 static inline void init_configuration(struct wiimote_ctl *wctl)
 135 {
 136         assert(wctl);
 137         memset(wctl, 0, sizeof(*wctl));
 138 }
 139
 140 static void clean_configuration(struct wiimote_ctl *wctl)
 141 {
 142         assert(wctl);
 143         free(wctl->address);
 144         free(wctl->device);
 145 }
 146
 147 static void wiimote_scan_devices(void)
 148 {
 149         int i;
 150
 151         int device_id, sock, len = 8, flags;
 152         int max_resp = 255, num_resp;
 153
 154         char address[19] = { 0 };
 155         char name[248]   = { 0 };
 156
 157         inquiry_info *ii = NULL;
 158
 159         printf("Scanning for Wiimote devices ...\n");
 160         printf("Set your Wiimote into discoverable mode (press 1 + 2).\n");
 161
 162         flags = IREQ_CACHE_FLUSH;
 163
 164         device_id = hci_get_route(NULL);
 165
 166         sock = hci_open_dev(device_id);
 167         if(device_id < 0 || sock < 0) {
 168                 perror("hci_open_dev");
 169                 exit(EXIT_FAILURE);
 170         }
 171
 172         ii = malloc(max_resp * sizeof(*ii));
 173         if(!ii) {
 174                 perror("malloc");
 175                 exit(EXIT_FAILURE);
 176         }
 177
 178         num_resp = hci_inquiry(device_id, len, max_resp, NULL, &ii, flags);
 179         if(num_resp < 0)
 180                 perror("hci_inquiry");
 181
 182         for(i = 0; i < num_resp; ++i) {
 183                 ba2str(&(ii + i)->bdaddr, address);
 184                 memset(name, 0, sizeof(name));
 185
 186                 if(hci_read_remote_name(sock, &(ii + i)->bdaddr, sizeof(name),
 187                    name, 0) < 0) {
 188                         strcpy(name, "[unknown]");
 189                 }
 190
 191                 if(strstr(name, "Nintendo") != NULL)
 192                         printf("%s -- %s\n", address, name);
 193         }
 194
 195         free(ii);
 196         close(sock);
 197 }
 198
 199 static void set_configuration(int argc, char **argv, struct wiimote_ctl *wctl)
 200 {
 201         int c, sl, opt_idx;
 202
 203         assert(argv);
 204         assert(wctl);
 205
 206         while ((c = getopt_long(argc, argv, "a:d:svh", long_options,
 207                                 &opt_idx)) != EOF) {
 208                 switch (c) {
 209                 case 'h':
 210                         help();
 211                         break;
 212                 case 'v':
 213                         version();
 214                         break;
 215                 case 'a':
 216                         /* Allowed: XX:XX:XX:XX:XX:XX */
 217                         sl = strlen(optarg);
 218                         if(sl != 17) {
 219                                 fprintf(stderr, "Wrong address format!\n");
 220                                 help();
 221                         }
 222
 223                         wctl->address = strdup(optarg);
 224                         if(!wctl->address) {
 225                                 perror("strdup");
 226                                 help();
 227                         }
 228                         break;
 229                 case 'd':
 230                         wctl->device = strdup(optarg);
 231                         if(!wctl->device) {
 232                                 perror("strdup");
 233                                 help();
 234                         }
 235                         break;
 236                 case 's':
 237                         wiimote_scan_devices();
 238                         exit(EXIT_SUCCESS);
 239                         break;
 240                 case '?':
 241                         switch (optopt) {
 242                         case 'a':
 243                         case 'd':
 244                                 printf("Option -%c requires an argument!\n",
 245                                        optopt);
 246                                 help();
 247                         default:
 248                                 if(isprint(optopt)) {
 249                                         printf("Unknown option character "
 250                                                "`0x%X\'!\n", optopt);
 251                                 }
 252                                 help();
 253                         }
 254
 255                         return;
 256                 default:
 257                         abort();
 258                 }
 259         }
 260 }
 261
 262 static void check_configuration(struct wiimote_ctl *wctl)
 263 {
 264         assert(wctl);
 265         if(!wctl->address || !wctl->device) {
 266                 clean_configuration(wctl);
 267                 help();
 268         }
 269 }
 270
 271 static void signal_action(int s)
 272 {
 273         sigint = (s == SIGINT);
 274 }
 275
 276 static void set_signalhandler(void)
 277 {
 278         sigset_t block_mask;
 279         struct sigaction usr_action;
 280
 281         sigfillset(&block_mask);
 282         usr_action.sa_handler = signal_action;
 283         usr_action.sa_mask = block_mask;
 284         usr_action.sa_flags = 0;
 285         sigaction(SIGINT, &usr_action, NULL);
 286 }
 287
 288 static int wiimote_init(struct wiimote_ctl *wctl)
 289 {
 290         assert(wctl);
 291
 292         wctl->fd_device = open(wctl->device, O_RDWR);
 293         if(wctl->fd_device < 0) {
 294                 perror("open");
 295                 exit(EXIT_FAILURE);
 296         }
 297
 298         wctl->sock_ctrl = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
 299         wctl->sock_intr = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
 300
 301         if(wctl->sock_ctrl < 0 || wctl->sock_intr < 0) {
 302                 perror("socket");
 303                 exit(EXIT_FAILURE);
 304         }
 305
 306         return 0;
 307 }
 308
 309 static void wiimote_cleanup(struct wiimote_ctl *wctl)
 310 {
 311         close(wctl->fd_device);
 312         close(wctl->sock_ctrl);
 313         close(wctl->sock_intr);
 314 }
 315
 316 static int wiimote_inject_config_data(struct wiimote_ctl *wctl)
 317 {
 318         int rc;
 319         uint8_t calibration_data[] = WII_SET_CALIBRATION_DATA;
 320         uint8_t enable_ba_data[]   = WII_ENABLE_BUTTON_ACCELM;
 321
 322         assert(wctl);
 323
 324         rc = write(wctl->sock_ctrl, calibration_data, sizeof(calibration_data));
 325         if(rc < 0) {
 326                 perror("write");
 327                 return rc;
 328         }
 329
 330         rc = write(wctl->sock_ctrl, enable_ba_data, sizeof(enable_ba_data));
 331         if(rc < 0) {
 332                 perror("write");
 333                 return rc;
 334         }
 335
 336         return 0;
 337 }
 338
 339 static int wiimote_connect_intr_channel(struct wiimote_ctl *wctl)
 340 {
 341         struct sockaddr_l2 addr_intr;
 342
 343         assert(wctl);
 344
 345         addr_intr.l2_family = AF_BLUETOOTH;
 346         addr_intr.l2_psm    = htobs(L2CAP_PSM_HIDP_INTR);
 347
 348         str2ba(wctl->address, &addr_intr.l2_bdaddr);
 349
 350 //      return connect(wctl->sock_intr, (struct sockaddr *) &addr_intr,
 351 //                     sizeof(addr_intr));
 352         errno = 0;
 353         wctl->sock_intr = l2cap_connect(BDADDR_ANY, &addr_intr.l2_bdaddr, L2CAP_PSM_HIDP_INTR);
 354         perror("l2cap_connect intr");
 355         return (wctl->sock_intr < 0);
 356 }
 357
 358 static int wiimote_connect_ctrl_channel(struct wiimote_ctl *wctl)
 359 {
 360         struct sockaddr_l2 addr_ctrl;
 361
 362         assert(wctl);
 363
 364         addr_ctrl.l2_family = AF_BLUETOOTH;
 365         addr_ctrl.l2_psm    = htobs(L2CAP_PSM_HIDP_CTRL);
 366
 367         str2ba(wctl->address, &addr_ctrl.l2_bdaddr);
 368
 369 //      return connect(wctl->sock_ctrl, (struct sockaddr *) &addr_ctrl,
 370 //                     sizeof(addr_ctrl));
 371         errno = 0;
 372         wctl->sock_ctrl = l2cap_connect(BDADDR_ANY, &addr_ctrl.l2_bdaddr, L2CAP_PSM_HIDP_CTRL);
 373         perror("l2cap_connect intr");
 374         return (wctl->sock_ctrl < 0);
 375 }
 376
 377 static int wiimote_connect_channels(struct wiimote_ctl *wctl)
 378 {
 379         int rc = 0;
 380
 381         assert(wctl);
 382
 383         rc = wiimote_connect_ctrl_channel(wctl);
 384         if(rc) { perror("ctrl channel");
 385                 return -EIO; }
 386         rc = wiimote_connect_intr_channel(wctl);
 387         if(rc) { perror("intr channel");
 388                 return -EIO; }
 389         rc = wiimote_inject_config_data(wctl);
 390         if(rc) { perror("conf data");
 391                 return rc; }
 392         return 0;
 393 }
 394
 395 static int wiimote_leds(struct wiimote_ctl *wctl, int l1, int l2, int l3, int l4)
 396 {
 397         int rc;
 398         uint8_t enable_leds[] = WII_GENERIC_LED;
 399
 400         assert(wctl);
 401
 402         if(l1)
 403                 enable_leds[2] |= 0x10;
 404         if(l2)
 405                 enable_leds[2] |= 0x20;
 406         if(l3)
 407                 enable_leds[2] |= 0x40;
 408         if(l4)
 409                 enable_leds[2] |= 0x80;
 410
 411         rc = write(wctl->sock_ctrl, enable_leds, sizeof(enable_leds));
 412         if(rc < 0) {
 413                 perror("write");
 414                 return rc;
 415         }
 416
 417         return 0;
 418 }
 419
 420 static int wiimote_rumble(struct wiimote_ctl *wctl, uint32_t usec)
 421 {
 422         int rc;
 423         uint8_t enable_rumble[]  = WII_ENABLE_RUMBLE;
 424         uint8_t disable_rumble[] = WII_DISABLE_RUMBLE;
 425
 426         assert(wctl);
 427
 428         rc = write(wctl->sock_ctrl, enable_rumble, sizeof(enable_rumble));
 429         if(rc < 0) {
 430                 perror("write");
 431                 return rc;
 432         }
 433
 434         usleep(usec);
 435
 436         rc = write(wctl->sock_ctrl, disable_rumble, sizeof(disable_rumble));
 437         if(rc < 0) {
 438                 perror("write");
 439                 return rc;
 440         }
 441
 442         return 0;
 443 }
 444
 445 static void *wiimote_rumble_thread(void *arg)
 446 {
 447         struct wiimote_ctl *wctl = (struct wiimote_ctl *) arg;
 448
 449         while(!sigint) {
 450                 wiimote_leds(wctl, 1, 0, 0, 0);
 451                 usleep(250000);
 452                 wiimote_leds(wctl, 0, 1, 0, 0);
 453                 usleep(250000);
 454                 wiimote_leds(wctl, 0, 0, 1, 0);
 455                 usleep(250000);
 456                 wiimote_leds(wctl, 0, 0, 0, 1);
 457                 usleep(250000);
 458                 wiimote_leds(wctl, 0, 0, 1, 0);
 459                 usleep(250000);
 460                 wiimote_leds(wctl, 0, 1, 0, 0);
 461                 usleep(250000);
 462                 wiimote_leds(wctl, 1, 0, 0, 0);
 463                 usleep(250000);
 464         }
 465
 466         pthread_exit(0);
 467 }
 468
 469 static int wiimote_read_sensors_loop(struct wiimote_ctl *wctl)
 470 {
 471         int rc;
 472
 473         float norm;
 474         float calx, caly, calz;
 475         float dx, dy, dz;
 476         float roll, pitch;
 477
 478         uint8_t buffer[WII_BLK_TRANSMISSION_LEN];
 479         struct wiimote_data wbuf;
 480
 481         assert(wctl);
 482
 483         /* Initial settings */
 484         calx = 122.f;
 485         caly = 129.f;
 486         calz = 123.f;
 487
 488         while(!sigint) {
 489                 usleep(WII_BLK_SLEEP_GAP_LEN);
 490
 491                 memset(buffer, 0, sizeof(buffer));
 492
 493                 rc = read(wctl->sock_intr, buffer, sizeof(buffer));
 494                 if(rc < 0) {
 495                         perror("read");
 496                         return rc;
 497                 }
 498
 499                 /* Check for valid buffer */
 500                 if(buffer[0] != 0xA1 && buffer[1] != 0x33)
 501                         continue;
 502
 503                 wbuf.buttons = ((uint16_t) ((buffer[2] & ~0x60) << 8) & 0xFF00) |
 504                                ((uint16_t)  (buffer[3] & ~0x60) & 0x00FF);
 505
 506                 /* Some bits are also encoded between button status bits */
 507                 dx = 1.0f * (buffer[4] | (buffer[2] & 0x60) >> 5) - calx;
 508                 dy = 1.0f * (buffer[5] | (buffer[3] & 0x20) >> 5) - caly;
 509                 dz = 1.0f * (buffer[6] | (buffer[3] & 0x40) >> 6) - calz;
 510
 511                 norm = 1.f / sqrtf(dx * dx + dy * dy + dz * dz);
 512
 513                 dx = 2.0f * atanf(dx * norm);
 514                 dy = 2.0f * atanf(dy * norm);
 515
 516                 roll  = dx * 180.0f / M_PI;
 517                 pitch = dy * 180.0f / M_PI;
 518
 519                 wbuf.pos_dx = 0;
 520                 if((int) roll > 30 || (int) roll < -30)
 521                         wbuf.pos_dx = (int) roll / 20;
 522
 523                 wbuf.pos_dy = 0;
 524                 if((int) pitch > 30 || (int) pitch < -30)
 525                         wbuf.pos_dy = (int) pitch / 20;
 526
 527                 wbuf.pos_dy *= WII_INVERT_Y_AXIS;
 528
 529                 rc = write(wctl->fd_device, &wbuf, sizeof(wbuf));
 530                 if(rc < 0){
 531                         perror("write");
 532                         return rc;
 533                 }
 534         }
 535
 536         return 0;
 537 }
 538
 539 int main(int argc, char **argv)
 540 {
 541         int rc;
 542         pthread_t rumble_thread;
 543         struct wiimote_ctl  wctl;
 544
 545         init_configuration(&wctl);
 546         set_configuration(argc, argv, &wctl);
 547         check_configuration(&wctl);
 548         set_signalhandler();
 549
 550         printf("Set your Wiimote (%s) into discoverable mode (press 1 + 2).\n",
 551                wctl.address);
 552
 553         wiimote_init(&wctl);
 554         while(wiimote_connect_channels(&wctl) != 0 && !sigint)
 555                 /* NOP */ ;
 556         wiimote_rumble(&wctl, 2000000);
 557
 558         rc = pthread_create(&rumble_thread, NULL, wiimote_rumble_thread, &wctl);
 559         if(rc) {
 560                 perror("pthread_create");
 561                 exit(EXIT_FAILURE);
 562         }
 563
 564         wiimote_read_sensors_loop(&wctl);
 565         wiimote_cleanup(&wctl);
 566
 567         clean_configuration(&wctl);
 568         return 0;
 569 }