plugins/sixaxis.c

   1 /*
   2  *
   3  *  BlueZ - Bluetooth protocol stack for Linux
   4  *
   5  *  Copyright (C) 2009  Bastien Nocera <hadess@hadess.net>
   6  *  Copyright (C) 2011  Antonio Ospite <ospite@studenti.unina.it>
   7  *  Copyright (C) 2013  Szymon Janc <szymon.janc@gmail.com>
   8  *
   9  *
  10  *  This program is free software; you can redistribute it and/or modify
  11  *  it under the terms of the GNU General Public License as published by
  12  *  the Free Software Foundation; either version 2 of the License, or
  13  *  (at your option) any later version.
  14  *
  15  *  This program is distributed in the hope that it will be useful,
  16  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
  17  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  18  *  GNU General Public License for more details.
  19  *
  20  *  You should have received a copy of the GNU General Public License
  21  *  along with this program; if not, write to the Free Software
  22  *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  23  *
  24  */
  25
  26 #ifdef HAVE_CONFIG_H
  27 #include <config.h>
  28 #endif
  29
  30 #include <stddef.h>
  31 #include <errno.h>
  32 #include <fcntl.h>
  33 #include <unistd.h>
  34 #include <stdlib.h>
  35 #include <sys/ioctl.h>
  36 #include <linux/hidraw.h>
  37 #include <linux/input.h>
  38 #include <glib.h>
  39 #include <libudev.h>
  40
  41 #include "lib/bluetooth.h"
  42 #include "lib/sdp.h"
  43 #include "lib/uuid.h"
  44
  45 #include "src/adapter.h"
  46 #include "src/device.h"
  47 #include "src/plugin.h"
  48 #include "src/log.h"
  49 #include "src/shared/util.h"
  50
  51 static const struct {
  52         const char *name;
  53         uint16_t source;
  54         uint16_t vid;
  55         uint16_t pid;
  56         uint16_t version;
  57 } devices[] = {
  58         {
  59                 .name = "PLAYSTATION(R)3 Controller",
  60                 .source = 0x0002,
  61                 .vid = 0x054c,
  62                 .pid = 0x0268,
  63                 .version = 0x0000,
  64         },
  65         {
  66                 .name = "Navigation Controller",
  67                 .source = 0x0002,
  68                 .vid = 0x054c,
  69                 .pid = 0x042f,
  70                 .version = 0x0000,
  71         },
  72 };
  73
  74 struct leds_data {
  75         char *syspath_prefix;
  76         uint8_t bitmap;
  77 };
  78
  79 static void leds_data_destroy(struct leds_data *data)
  80 {
  81         free(data->syspath_prefix);
  82         free(data);
  83 }
  84
  85 static struct udev *ctx = NULL;
  86 static struct udev_monitor *monitor = NULL;
  87 static guint watch_id = 0;
  88
  89 static int get_device_bdaddr(int fd, bdaddr_t *bdaddr)
  90 {
  91         uint8_t buf[18];
  92         int ret;
  93
  94         memset(buf, 0, sizeof(buf));
  95
  96         buf[0] = 0xf2;
  97
  98         ret = ioctl(fd, HIDIOCGFEATURE(sizeof(buf)), buf);
  99         if (ret < 0) {
 100                 error("sixaxis: failed to read device address (%s)",
 101                                                         strerror(errno));
 102                 return ret;
 103         }
 104
 105         baswap(bdaddr, (bdaddr_t *) (buf + 4));
 106
 107         return 0;
 108 }
 109
 110 static int get_master_bdaddr(int fd, bdaddr_t *bdaddr)
 111 {
 112         uint8_t buf[8];
 113         int ret;
 114
 115         memset(buf, 0, sizeof(buf));
 116
 117         buf[0] = 0xf5;
 118
 119         ret = ioctl(fd, HIDIOCGFEATURE(sizeof(buf)), buf);
 120         if (ret < 0) {
 121                 error("sixaxis: failed to read master address (%s)",
 122                                                         strerror(errno));
 123                 return ret;
 124         }
 125
 126         baswap(bdaddr, (bdaddr_t *) (buf + 2));
 127
 128         return 0;
 129 }
 130
 131 static int set_master_bdaddr(int fd, const bdaddr_t *bdaddr)
 132 {
 133         uint8_t buf[8];
 134         int ret;
 135
 136         buf[0] = 0xf5;
 137         buf[1] = 0x01;
 138
 139         baswap((bdaddr_t *) (buf + 2), bdaddr);
 140
 141         ret = ioctl(fd, HIDIOCSFEATURE(sizeof(buf)), buf);
 142         if (ret < 0)
 143                 error("sixaxis: failed to write master address (%s)",
 144                                                         strerror(errno));
 145
 146         return ret;
 147 }
 148
 149 static uint8_t calc_leds_bitmap(int number)
 150 {
 151         uint8_t bitmap = 0;
 152
 153         /* TODO we could support up to 10 (1 + 2 + 3 + 4) */
 154         if (number > 7)
 155                 return bitmap;
 156
 157         if (number > 4) {
 158                 bitmap |= 0x10;
 159                 number -= 4;
 160         }
 161
 162         bitmap |= 0x01 << number;
 163
 164         return bitmap;
 165 }
 166
 167 static void set_leds_hidraw(int fd, uint8_t leds_bitmap)
 168 {
 169         /*
 170          * the total time the led is active (0xff means forever)
 171          * |     duty_length: cycle time in deciseconds (0 - "blink very fast")
 172          * |     |     ??? (Maybe a phase shift or duty_length multiplier?)
 173          * |     |     |     % of duty_length led is off (0xff means 100%)
 174          * |     |     |     |     % of duty_length led is on (0xff means 100%)
 175          * |     |     |     |     |
 176          * 0xff, 0x27, 0x10, 0x00, 0x32,
 177          */
 178         uint8_t leds_report[] = {
 179                 0x01,
 180                 0x00, 0x00, 0x00, 0x00, 0x00, /* rumble values TBD */
 181                 0x00, 0x00, 0x00, 0x00, 0x00, /* LED_1=0x02, LED_2=0x04 ... */
 182                 0xff, 0x27, 0x10, 0x00, 0x32, /* LED_4 */
 183                 0xff, 0x27, 0x10, 0x00, 0x32, /* LED_3 */
 184                 0xff, 0x27, 0x10, 0x00, 0x32, /* LED_2 */
 185                 0xff, 0x27, 0x10, 0x00, 0x32, /* LED_1 */
 186                 0x00, 0x00, 0x00, 0x00, 0x00,
 187         };
 188         int ret;
 189
 190         leds_report[10] = leds_bitmap;
 191
 192         ret = write(fd, leds_report, sizeof(leds_report));
 193         if (ret == sizeof(leds_report))
 194                 return;
 195
 196         if (ret < 0)
 197                 error("sixaxis: failed to set LEDS (%s)", strerror(errno));
 198         else
 199                 error("sixaxis: failed to set LEDS (%d bytes written)", ret);
 200 }
 201
 202 static bool set_leds_sysfs(struct leds_data *data)
 203 {
 204         int i;
 205
 206         if (!data->syspath_prefix)
 207                 return false;
 208
 209         /* start from 1, LED0 is never used */
 210         for (i = 1; i <= 4; i++) {
 211                 char path[PATH_MAX] = { 0 };
 212                 char buf[2] = { 0 };
 213                 int fd;
 214                 int ret;
 215
 216                 snprintf(path, PATH_MAX, "%s%d/brightness",
 217                                                 data->syspath_prefix, i);
 218
 219                 fd = open(path, O_WRONLY);
 220                 if (fd < 0) {
 221                         error("sixaxis: cannot open %s (%s)", path,
 222                                                         strerror(errno));
 223                         return false;
 224                 }
 225
 226                 buf[0] = '0' + !!(data->bitmap & (1 << i));
 227                 ret = write(fd, buf, sizeof(buf));
 228                 close(fd);
 229                 if (ret != sizeof(buf))
 230                         return false;
 231         }
 232
 233         return true;
 234 }
 235
 236 static gboolean setup_leds(GIOChannel *channel, GIOCondition cond,
 237                                                         gpointer user_data)
 238 {
 239         struct leds_data *data = user_data;
 240
 241         if (!data)
 242                 return FALSE;
 243
 244         if (cond & (G_IO_HUP | G_IO_ERR | G_IO_NVAL))
 245                 goto out;
 246
 247         if(!set_leds_sysfs(data)) {
 248                 int fd = g_io_channel_unix_get_fd(channel);
 249                 set_leds_hidraw(fd, data->bitmap);
 250         }
 251
 252 out:
 253         leds_data_destroy(data);
 254
 255         return FALSE;
 256 }
 257
 258 static bool setup_device(int fd, int index, struct btd_adapter *adapter)
 259 {
 260         char device_addr[18], master_addr[18], adapter_addr[18];
 261         bdaddr_t device_bdaddr, master_bdaddr;
 262         const bdaddr_t *adapter_bdaddr;
 263         struct btd_device *device;
 264
 265         if (get_device_bdaddr(fd, &device_bdaddr) < 0)
 266                 return false;
 267
 268         if (get_master_bdaddr(fd, &master_bdaddr) < 0)
 269                 return false;
 270
 271         /* This can happen if controller was plugged while already connected
 272          * eg. to charge up battery.
 273          * Don't set LEDs in that case, hence return false */
 274         device = btd_adapter_find_device(adapter, &device_bdaddr,
 275                                                         BDADDR_BREDR);
 276         if (device && btd_device_is_connected(device))
 277                 return false;
 278
 279         adapter_bdaddr = btd_adapter_get_address(adapter);
 280
 281         if (bacmp(adapter_bdaddr, &master_bdaddr)) {
 282                 if (set_master_bdaddr(fd, adapter_bdaddr) < 0)
 283                         return false;
 284         }
 285
 286         ba2str(&device_bdaddr, device_addr);
 287         ba2str(&master_bdaddr, master_addr);
 288         ba2str(adapter_bdaddr, adapter_addr);
 289         DBG("remote %s old_master %s new_master %s",
 290                                 device_addr, master_addr, adapter_addr);
 291
 292         device = btd_adapter_get_device(adapter, &device_bdaddr, BDADDR_BREDR);
 293
 294         if (g_slist_find_custom(btd_device_get_uuids(device), HID_UUID,
 295                                                 (GCompareFunc)strcasecmp)) {
 296                 DBG("device %s already known, skipping", device_addr);
 297                 return true;
 298         }
 299
 300         info("sixaxis: setting up new device");
 301
 302         btd_device_device_set_name(device, devices[index].name);
 303         btd_device_set_pnpid(device, devices[index].source, devices[index].vid,
 304                                 devices[index].pid, devices[index].version);
 305         btd_device_set_temporary(device, false);
 306
 307         return true;
 308 }
 309
 310 static int get_js_number(struct udev_device *udevice)
 311 {
 312         struct udev_list_entry *devices, *dev_list_entry;
 313         struct udev_enumerate *enumerate;
 314         struct udev_device *hid_parent;
 315         const char *hidraw_node;
 316         const char *hid_id;
 317         int number = 0;
 318
 319         hid_parent = udev_device_get_parent_with_subsystem_devtype(udevice,
 320                                                                 "hid", NULL);
 321
 322         /*
 323          * Look for HID_UNIQ first for the correct behavior via BT, if
 324          * HID_UNIQ is not available it means the USB bus is being used and we
 325          * can rely on HID_PHYS.
 326          */
 327         hid_id = udev_device_get_property_value(hid_parent, "HID_UNIQ");
 328         if (!hid_id)
 329                 hid_id = udev_device_get_property_value(hid_parent,
 330                                                         "HID_PHYS");
 331
 332         hidraw_node = udev_device_get_devnode(udevice);
 333         if (!hid_id || !hidraw_node)
 334                 return 0;
 335
 336         enumerate = udev_enumerate_new(udev_device_get_udev(udevice));
 337         udev_enumerate_add_match_sysname(enumerate, "js*");
 338         udev_enumerate_scan_devices(enumerate);
 339         devices = udev_enumerate_get_list_entry(enumerate);
 340
 341         udev_list_entry_foreach(dev_list_entry, devices) {
 342                 struct udev_device *input_parent;
 343                 struct udev_device *js_dev;
 344                 const char *input_id;
 345                 const char *devname;
 346
 347                 devname = udev_list_entry_get_name(dev_list_entry);
 348                 js_dev = udev_device_new_from_syspath(
 349                                                 udev_device_get_udev(udevice),
 350                                                 devname);
 351
 352                 input_parent = udev_device_get_parent_with_subsystem_devtype(
 353                                                         js_dev, "input", NULL);
 354                 if (!input_parent)
 355                         goto next;
 356
 357                 /* check if this is the joystick relative to the hidraw device
 358                  * above */
 359                 input_id = udev_device_get_sysattr_value(input_parent, "uniq");
 360
 361                 /*
 362                  * A strlen() check is needed because input device over USB
 363                  * have the UNIQ attribute defined but with an empty value.
 364                  */
 365                 if (!input_id || strlen(input_id) == 0)
 366                         input_id = udev_device_get_sysattr_value(input_parent,
 367                                                                  "phys");
 368
 369                 if (!input_id)
 370                         goto next;
 371
 372                 if (!strcmp(input_id, hid_id)) {
 373                         number = atoi(udev_device_get_sysnum(js_dev));
 374
 375                         /* joystick numbers start from 0, leds from 1 */
 376                         number++;
 377
 378                         udev_device_unref(js_dev);
 379                         break;
 380                 }
 381 next:
 382                 udev_device_unref(js_dev);
 383         }
 384
 385         udev_enumerate_unref(enumerate);
 386
 387         return number;
 388 }
 389
 390 static char *get_leds_syspath_prefix(struct udev_device *udevice)
 391 {
 392         struct udev_list_entry *dev_list_entry;
 393         struct udev_enumerate *enumerate;
 394         struct udev_device *hid_parent;
 395         const char *syspath;
 396         char *syspath_prefix;
 397
 398         hid_parent = udev_device_get_parent_with_subsystem_devtype(udevice,
 399                                                                 "hid", NULL);
 400
 401         enumerate = udev_enumerate_new(udev_device_get_udev(udevice));
 402         udev_enumerate_add_match_parent(enumerate, hid_parent);
 403         udev_enumerate_add_match_subsystem(enumerate, "leds");
 404         udev_enumerate_scan_devices(enumerate);
 405
 406         dev_list_entry = udev_enumerate_get_list_entry(enumerate);
 407         if (!dev_list_entry) {
 408                 syspath_prefix = NULL;
 409                 goto out;
 410         }
 411
 412         syspath = udev_list_entry_get_name(dev_list_entry);
 413
 414         /*
 415          * All the sysfs paths of the LEDs have the same structure, just the
 416          * number changes, so strip it and store only the common prefix.
 417          *
 418          * Subtracting 1 here means assuming that the LED number is a single
 419          * digit, this is safe as the kernel driver only exposes 4 LEDs.
 420          */
 421         syspath_prefix = strndup(syspath, strlen(syspath) - 1);
 422
 423 out:
 424         udev_enumerate_unref(enumerate);
 425
 426         return syspath_prefix;
 427 }
 428
 429 static struct leds_data *get_leds_data(struct udev_device *udevice)
 430 {
 431         struct leds_data *data;
 432         int number;
 433
 434         number = get_js_number(udevice);
 435         DBG("number %d", number);
 436
 437         data = malloc0(sizeof(*data));
 438         if (!data)
 439                 return NULL;
 440
 441         data->bitmap = calc_leds_bitmap(number);
 442         if (data->bitmap == 0) {
 443                 leds_data_destroy(data);
 444                 return NULL;
 445         }
 446
 447         /*
 448          * It's OK if this fails, set_leds_hidraw() will be used in
 449          * case data->syspath_prefix is NULL.
 450          */
 451         data->syspath_prefix = get_leds_syspath_prefix(udevice);
 452
 453         return data;
 454 }
 455
 456 static int get_supported_device(struct udev_device *udevice, uint16_t *bus)
 457 {
 458         struct udev_device *hid_parent;
 459         uint16_t vid, pid;
 460         const char *hid_id;
 461         guint i;
 462
 463         hid_parent = udev_device_get_parent_with_subsystem_devtype(udevice,
 464                                                                 "hid", NULL);
 465         if (!hid_parent)
 466                 return -1;
 467
 468         hid_id = udev_device_get_property_value(hid_parent, "HID_ID");
 469
 470         if (sscanf(hid_id, "%hx:%hx:%hx", bus, &vid, &pid) != 3)
 471                 return -1;
 472
 473         for (i = 0; i < G_N_ELEMENTS(devices); i++) {
 474                 if (devices[i].vid == vid && devices[i].pid == pid)
 475                         return i;
 476         }
 477
 478         return -1;
 479 }
 480
 481 static void device_added(struct udev_device *udevice)
 482 {
 483         struct btd_adapter *adapter;
 484         GIOChannel *io;
 485         uint16_t bus;
 486         int index;
 487         int fd;
 488
 489         adapter = btd_adapter_get_default();
 490         if (!adapter)
 491                 return;
 492
 493         index = get_supported_device(udevice, &bus);
 494         if (index < 0)
 495                 return;
 496
 497         info("sixaxis: compatible device connected: %s (%04X:%04X)",
 498                                 devices[index].name, devices[index].vid,
 499                                 devices[index].pid);
 500
 501         fd = open(udev_device_get_devnode(udevice), O_RDWR);
 502         if (fd < 0)
 503                 return;
 504
 505         io = g_io_channel_unix_new(fd);
 506
 507         switch (bus) {
 508         case BUS_USB:
 509                 if (!setup_device(fd, index, adapter))
 510                         break;
 511
 512                 /* fall through */
 513         case BUS_BLUETOOTH:
 514                 /* wait for events before setting leds */
 515                 g_io_add_watch(io, G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
 516                                 setup_leds, get_leds_data(udevice));
 517
 518                 break;
 519         default:
 520                 DBG("uknown bus type (%u)", bus);
 521                 break;
 522         }
 523
 524         g_io_channel_set_close_on_unref(io, TRUE);
 525         g_io_channel_unref(io);
 526 }
 527
 528 static gboolean monitor_watch(GIOChannel *source, GIOCondition condition,
 529                                                         gpointer data)
 530 {
 531         struct udev_device *udevice;
 532
 533         udevice = udev_monitor_receive_device(monitor);
 534         if (!udevice)
 535                 return TRUE;
 536
 537         if (!g_strcmp0(udev_device_get_action(udevice), "add"))
 538                 device_added(udevice);
 539
 540         udev_device_unref(udevice);
 541
 542         return TRUE;
 543 }
 544
 545 static int sixaxis_init(void)
 546 {
 547         GIOChannel *channel;
 548
 549         DBG("");
 550
 551         ctx = udev_new();
 552         if (!ctx)
 553                 return -EIO;
 554
 555         monitor = udev_monitor_new_from_netlink(ctx, "udev");
 556         if (!monitor) {
 557                 udev_unref(ctx);
 558                 ctx = NULL;
 559
 560                 return -EIO;
 561         }
 562
 563         /* Listen for newly connected hidraw interfaces */
 564         udev_monitor_filter_add_match_subsystem_devtype(monitor, "hidraw",
 565                                                                         NULL);
 566         udev_monitor_enable_receiving(monitor);
 567
 568         channel = g_io_channel_unix_new(udev_monitor_get_fd(monitor));
 569         watch_id = g_io_add_watch(channel, G_IO_IN, monitor_watch, NULL);
 570         g_io_channel_unref(channel);
 571
 572         return 0;
 573 }
 574
 575 static void sixaxis_exit(void)
 576 {
 577         DBG("");
 578
 579         g_source_remove(watch_id);
 580         watch_id = 0;
 581
 582         udev_monitor_unref(monitor);
 583         monitor = NULL;
 584
 585         udev_unref(ctx);
 586         ctx = NULL;
 587 }
 588
 589 BLUETOOTH_PLUGIN_DEFINE(sixaxis, VERSION, BLUETOOTH_PLUGIN_PRIORITY_LOW,
 590                                                 sixaxis_init, sixaxis_exit)