Set SixAxis Bluetooth Pair

sixpair.c

/**
 * Author: [email protected]
 * Credits:
 *    1. https://help.ubuntu.com/community/Sixaxis?action=AttachFile&do=get&target=sixpair.c
 *    2. https://github.com/kLeZ/SixPair
 *
 * Compile with:
 *    clang -Weverything -o sixpairng sixpairng.c -lusb-1.0
 *      OR
 *    gcc -o sixpairng sixpairng.c -lusb-1.0
 *
 *  Usage:
 *    sixpairng
 *    sixpairng -h
 *    sixpairng -s
 *    sixpairng ##:##:##:##:##:##
 *
 */

#include <stdio.h>
#include <assert.h>
#include <errno.h>
#include <fcntl.h>
#include <string.h>
#include <unistd.h>
#include <libusb-1.0/libusb.h>
#include <stdlib.h>

#define VENDOR 0x054c
#define PRODUCT 0x0268

static char iBTAddr[18];
static int conversion_error = 0;

static libusb_device* find_dualshock3(libusb_device **devs, ssize_t max_devices);
static void process_device(libusb_device *dev, int argc, char **argv);
static unsigned char convert_to_unsigned_short(unsigned long val);
static void get_host_bt_addr(void);

int main(int argc, char *argv[])
{
  ssize_t cnt;
  libusb_device **devs, *ds3;
  int r = libusb_init(NULL);
  if (r < 0)
    perror("Error libusb_init");

  // Enumerate devices
  cnt = libusb_get_device_list(NULL, &devs);
  if (cnt < 0) {
    libusb_exit(NULL);
    return 1;
  }

  get_host_bt_addr();
  ds3 = find_dualshock3(devs, cnt);
  if (ds3 == NULL) {
    printf("Dualshock3 not found.\n");
    return -1;
  }

  process_device(ds3, argc, argv);

  libusb_free_device_list(devs, 1);
  libusb_exit(NULL);
  return 0;
}

static void process_device(libusb_device *dev, int argc, char **argv)
{
  libusb_device_handle *hds3;
  struct libusb_device_descriptor desc;
  int ret = libusb_get_device_descriptor(dev, &desc);
  unsigned char masterBtAddr[8] = {0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
  unsigned char mAddr[8];
  memset(mAddr, 0, sizeof(mAddr));
  uint16_t itfNum = 0;  // For DualShock3, interface number is 0
  if (ret < 0) {
    printf("Error (%d): %s\n", ret, libusb_strerror(ret));
    return ;
  }

  ret = libusb_open(dev, &hds3);
  if (ret < 0) {
    printf("Error (%d): %s\n", ret, libusb_strerror(ret));
    return ;
  }

  libusb_detach_kernel_driver(hds3, itfNum);
  ret = libusb_claim_interface(hds3, itfNum);
  if (ret < 0) {
    printf("Error claim_interface (%d): %s\n", ret, libusb_strerror(ret));
    return ;
  }

  // Show bluetooth master
  ret = libusb_control_transfer(
          hds3,
          LIBUSB_ENDPOINT_IN | LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
          0x01,
          0x03f5,
          itfNum,
          (void*)mAddr,
          sizeof(mAddr),
          5000
        );

  if (ret < 0) {
    printf("Error REQ_GET_CONFIGURATION (%d): %s\n", ret, libusb_strerror(ret));
    return ;
  }

  printf("Current Bluetooth master: ");
  printf("%02x:%02x:%02x:%02x:%02x:%02x\n", mAddr[2], mAddr[3], mAddr[4], mAddr[5], mAddr[6], mAddr[7]);

  if (argc > 1) {
    if (memcmp("-h", argv[1], 2) == 0) {
      printf("Usage: sixpairng ##:##:##:##:##:##\n");
    } else if (memcmp("-s", argv[1], 2) == 0) {
      masterBtAddr[2] = convert_to_unsigned_short(strtoul(strtok(iBTAddr, ":"), NULL, 16));
      masterBtAddr[3] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[4] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[5] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[6] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[7] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
    } else {
      masterBtAddr[2] = convert_to_unsigned_short(strtoul(strtok(argv[1], ":"), NULL, 16));
      masterBtAddr[3] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[4] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[5] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[6] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));
      masterBtAddr[7] = convert_to_unsigned_short(strtoul(strtok(NULL, ":"), NULL, 16));

      if (conversion_error == 1) {
        printf("Error invalid bluetooth address.\n");
        goto cleanup;
      }
    }

    // Set bluetooth master
    printf("Set Bluetooth master to: ");
    printf("%02x:%02x:%02x:%02x:%02x:%02x\n", masterBtAddr[2], masterBtAddr[3], masterBtAddr[4], masterBtAddr[5], masterBtAddr[6], masterBtAddr[7]);
    ret = libusb_control_transfer(
            hds3,
            LIBUSB_ENDPOINT_OUT | LIBUSB_REQUEST_TYPE_CLASS | LIBUSB_RECIPIENT_INTERFACE,
            0x09,
            0x03f5,
            itfNum,
            masterBtAddr,
            sizeof(masterBtAddr),
            5000
          );

    if (ret < 0) {
      printf("Error REQ_SET_CONFIGURATION (%d): %s\n", ret, libusb_strerror(ret));
      return ;
    }
  }

cleanup:
  ret = libusb_release_interface(hds3, itfNum);
  assert(ret == 0);
  ret = libusb_attach_kernel_driver(hds3, itfNum);
  assert(ret == 0);
  libusb_close(hds3);
}

static libusb_device* find_dualshock3(libusb_device **devs, ssize_t max_devices)
{
  struct libusb_device_descriptor desc;
  int i, ret;
  for (i = 0; i < max_devices; i++) {
    ret = libusb_get_device_descriptor(devs[i], &desc);
    if (ret < 0) {
      printf("Error (%d): %s\n", ret, libusb_strerror(ret));
      return NULL;
    }
    if (desc.idVendor == VENDOR && desc.idProduct == PRODUCT) {
      return devs[i];
    }
  }
  return NULL;
}

static void get_host_bt_addr()
{
  int retlen;
  FILE *f = popen("bluetoothctl show | grep 'Controller' | awk '{print $2}'", "r");
  assert(f != NULL);
  retlen = fscanf(f, "%s", (char *)&iBTAddr);
  assert(retlen > 0);
  pclose(f);
}

static unsigned char convert_to_unsigned_short(unsigned long val)
{
  if (val > UINT8_MAX) {
    conversion_error = 1;
    return 1;
  }

  return (val & (unsigned long)UINT8_MAX);
}