sixpair/sixpair.c at main · redwil/sixpair · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
/*
 * sixpair.c version 2007-04-18
 * Modified to run on Mac OSX by
 * Erlend Cleveland 2011-09-04
 *
 * Compile with: gcc -o sixpair sixpair.c -lusb
 */

#include <string.h>
#include <unistd.h>
#include <stdio.h>
#include <usb.h>

#define VID 0x054C
#define PS3_PID 0x0268
#define PS4_PID 0x05C4
#define NAV_PID 0x042F

#define USB_DIR_IN 0x80
#define USB_DIR_OUT 0

void fatal(char *msg) { perror(msg); exit(1); }

void show_master(usb_dev_handle *devh, int itfnum, int pid) {
  int value = pid == PS4_PID ? 0x0312 : 0x03f5;

  printf("Current Bluetooth master: ");
  unsigned char msg[50];
  int res = usb_control_msg
    (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE,
     0x01, value, itfnum, (void*)msg, sizeof(msg), 5000);
  if ( res < 0 ) { perror("USB_REQ_GET_CONFIGURATION"); return; }

  if (pid == PS4_PID) {
    printf("%02x:%02x:%02x:%02x:%02x:%02x\n",
           msg[15], msg[14], msg[13], msg[12], msg[11], msg[10]);
  }
  else {
    printf("%02x:%02x:%02x:%02x:%02x:%02x\n",
           msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]);
  }
}

void set_master(usb_dev_handle *devh, int itfnum, int mac[6], int pid) {
  printf("Setting master bd_addr to %02x:%02x:%02x:%02x:%02x:%02x\n",
	 mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]);

  int res = -1;
  if (pid == PS4_PID) {
    unsigned char msg[23] = { 0x13, mac[5], mac[4], mac[3], mac[2], mac[1], mac[0], 0x56, 0xE8, 0x81, 0x38, 0x08, 0x06, 0x51, 0x41, 0xC0, 0x7F, 0x12, 0xAA, 0xD9, 0x66, 0x3C, 0xCE };

    res = usb_control_msg(devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 0x09, 0x0313, itfnum, (char*)msg, sizeof(msg), 5000);
  }
  else {
    unsigned char msg[8] = { 0x01, 0x00, mac[0], mac[1], mac[2], mac[3], mac[4], mac[5] };

    res = usb_control_msg(devh, USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 0x09, 0x03f5, itfnum, (char*)msg, sizeof(msg), 5000);
  }

  if ( res < 0 ) fatal("USB_REQ_SET_CONFIGURATION");
}

void process_device(int argc, char **argv, struct usb_device *dev,
		    struct usb_config_descriptor *cfg, int itfnum) {
  int mac[6], have_mac=0;

  usb_dev_handle *devh = usb_open(dev);
  if ( ! devh ) fatal("usb_open");

//  usb_detach_kernel_driver_np(devh, itfnum);

//  int res = usb_claim_interface(devh, itfnum);
//  if ( res < 0 ) fatal("usb_claim_interface");

  show_master(devh, itfnum, dev->descriptor.idProduct);

  if ( argc < 2 || sscanf(argv[1], "%x:%x:%x:%x:%x:%x",
		   &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) {

      printf("usage: %s [<bd_addr of master>]\n", argv[0]);
      exit(1);
  }
  set_master(devh, itfnum, mac, dev->descriptor.idProduct);

  usb_close(devh);
}

int main(int argc, char *argv[]) {

  usb_init();
  if ( usb_find_busses() < 0 ) fatal("usb_find_busses");
  if ( usb_find_devices() < 0 ) fatal("usb_find_devices");
  struct usb_bus *busses = usb_get_busses();
  if ( ! busses ) fatal("usb_get_busses");

  int found = 0;

  struct usb_bus *bus;
  for ( bus=busses; bus; bus=bus->next ) {
    struct usb_device *dev;
    for ( dev=bus->devices; dev; dev=dev->next) {
      struct usb_config_descriptor *cfg;
      for ( cfg = dev->config;
	    cfg < dev->config + dev->descriptor.bNumConfigurations;
	    ++cfg ) {

        if (cfg == NULL)
          continue;

	int itfnum;
	for ( itfnum=0; itfnum<cfg->bNumInterfaces; ++itfnum ) {
	  struct usb_interface *itf = &cfg->interface[itfnum];
	  struct usb_interface_descriptor *alt;
	  for ( alt = itf->altsetting;
		alt < itf->altsetting + itf->num_altsetting;
		++alt ) {
	    if ( dev->descriptor.idVendor == VID &&
            (dev->descriptor.idProduct == PS3_PID || dev->descriptor.idProduct == PS4_PID || dev->descriptor.idProduct == NAV_PID) &&
		 alt->bInterfaceClass == 3 ) {
	      process_device(argc, argv, dev, cfg, itfnum);
	      ++found;
	    }
	  }
	}
      }
    }
  }

  if ( ! found ) printf("No controller found on USB busses.\n");
  return 0;

}