/*
* command_complete():
*
* Called by HCI when an issued command has completed during the initialization of the
* host controller. Waits for a connection from remote device once connected.
*
* Event Sequence:
* HCI Reset -> Read Buf Size -> Read BDAddr -> Set Ev Filter -+
* +-----------------------------------------------------------+
* |_/-> Write CoD -> Cng Local Name -> Write Pg Timeout -> Inq -> Complete
* \-> Scan Enable -> Complete
*/
err_t command_complete(void *arg, struct hci_pcb *pcb, u8_t ogf, u8_t ocf, u8_t result)
{
u8_t cod_spp[] = {0x80,0x08,0x04};
u8_t devname[] = "PIC24F------";
u8_t n1, n2, n3;
u8_t flag = HCI_SET_EV_FILTER_AUTOACC_ROLESW;
switch(ogf) {
case HCI_INFO_PARAM:
switch(ocf) {
case HCI_READ_BUFFER_SIZE:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("successful HCI_READ_BUFFER_SIZE.\n"));
hci_read_bd_addr(read_bdaddr_complete);
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_READ_BUFFER_SIZE.\n"));
return ERR_CONN;
}
break;
case HCI_READ_BD_ADDR:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("successful HCI_READ_BD_ADDR.\n"));
/* Make discoverable */
hci_set_event_filter(HCI_SET_EV_FILTER_CONNECTION,
HCI_SET_EV_FILTER_ALLDEV, &flag);
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_READ_BD_ADDR.\n"));
return ERR_CONN;
}
break;
default:
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unknown HCI_INFO_PARAM command complete event\n"));
break;
}
break;
case HCI_HC_BB_OGF:
switch(ocf) {
case HCI_RESET:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("successful HCI_RESET.\n"));
hci_read_buffer_size();
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_RESET.\n"));
return ERR_CONN;
}
break;
case HCI_WRITE_SCAN_ENABLE:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("successful HCI_WRITE_SCAN_ENABLE.\n"));
hci_write_page_timeout(0x4000); /* 10.24s */
//hci_cmd_complete(NULL); /* Initialization done, don't come back */
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_WRITE_SCAN_ENABLE.\n"));
return ERR_CONN;
}
break;
case HCI_SET_EVENT_FILTER:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("successful HCI_SET_EVENT_FILTER.\n"));
hci_write_cod(cod_spp);
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_SET_EVENT_FILTER.\n"));
return ERR_CONN;
}
break;
case HCI_CHANGE_LOCAL_NAME:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Successful HCI_CHANGE_LOCAL_NAME.\n"));
hci_write_scan_enable(HCI_SCAN_EN_INQUIRY | HCI_SCAN_EN_PAGE);
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_CHANGE_LOCAL_NAME.\n"));
return ERR_CONN;
}
break;
case HCI_WRITE_COD:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Successful HCI_WRITE_COD.\n"));
n1 = (u8_t)(bt_spp_state.bdaddr.addr[0] / 100);
n2 = (u8_t)(bt_spp_state.bdaddr.addr[0] / 10) - n1 * 10;
n3 = bt_spp_state.bdaddr.addr[0] - n1 * 100 - n2 * 10;
devname[9] = '0' + n1;
devname[10] = '0' + n2;
devname[11] = '0' + n3;
hci_change_local_name(devname, sizeof(devname));
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_WRITE_COD.\n"));
return ERR_CONN;
}
break;
case HCI_WRITE_PAGE_TIMEOUT:
if(result == HCI_SUCCESS) {
LWIP_DEBUGF(BT_SPP_DEBUG, ("successful HCI_WRITE_PAGE_TIMEOUT.\n"));
//hci_cmd_complete(NULL); /* Initialization done, don't come back */
hci_connection_complete(acl_conn_complete);
LWIP_DEBUGF(BT_SPP_DEBUG, ("Initialization done.\n"));
//LWIP_DEBUGF(BT_SPP_DEBUG, ("Discover other Bluetooth devices.\n"));
//hci_inquiry(0x009E8B33, 0x04, 0x01, inquiry_complete); //FAILED????
} else {
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unsuccessful HCI_WRITE_PAGE_TIMEOUT.\n"));
return ERR_CONN;
}
break;
default:
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unknown HCI_HC_BB_OGF command complete event\n"));
break;
}
break;
default:
LWIP_DEBUGF(BT_SPP_DEBUG, ("Unknown command complete event. OGF = 0x%x OCF = 0x%x\n", ogf, ocf));
break;
}
return ERR_OK;
}
static int generic_reset_device(int dd)
{
bdaddr_t bdaddr;
int err;
err = hci_send_cmd(dd, 0x03, 0x0003, 0, NULL);
if (err < 0)
return err;
return hci_read_bd_addr(dd, &bdaddr, 10000);
}
const char *CBlueZStack::GetLocalAddress()
{
static char address[13];
bdaddr_t ba;
if (hci_read_bd_addr(m_fd, &ba, BLUEZ_TIMEOUT) < 0) {
return NULL;
}
sprintf(address, "%02X%02X%02X%02X%02X%02X", ba.b[5], ba.b[4], ba.b[3],
ba.b[2], ba.b[1], ba.b[0]);
return address;
}
/*
* \brief This function gets the bluetooth device address for a given device number.
*
* \param device_number the device number
* \param bdaddr the buffer to store the bluetooth device address
*
* \return 0 if successful, -1 otherwise
*/
int bt_get_device_bdaddr(int device_number, char bdaddr[18])
{
int ret = 0;
bdaddr_t bda;
int s = hci_open_dev (device_number);
if(hci_read_bd_addr(s, &bda, HCI_REQ_TIMEOUT) < 0)
{
ret = -1;
}
else
{
ba2str(&bda, bdaddr);
}
close(s);
return ret;
}
VALUE method_devices()
{
struct hci_dev_list_req *dl;
struct hci_dev_req *dr;
struct hci_dev_info di;
int i;
int ctl;
VALUE devices;
if (!(dl = malloc(HCI_MAX_DEV * sizeof(struct hci_dev_req) + sizeof(uint16_t)))) {
rb_raise(rb_eException, "Can't allocate memory");
return Qnil;
}
dl->dev_num = HCI_MAX_DEV;
dr = dl->dev_req;
ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (ioctl(ctl, HCIGETDEVLIST, (void *) dl) < 0) {
rb_raise(rb_eException, "Can't get device list");
return Qnil;
}
devices = rb_ary_new();
for (i = 0; i< dl->dev_num; i++) {
di.dev_id = (dr+i)->dev_id;
if (ioctl(ctl, HCIGETDEVINFO, (void *) &di) < 0)
continue;
if (hci_test_bit(HCI_RAW, &di.flags) &&
!bacmp(&di.bdaddr, BDADDR_ANY)) {
int dd = hci_open_dev(di.dev_id);
hci_read_bd_addr(dd, &di.bdaddr, 1000);
hci_close_dev(dd);
}
rb_ary_push(devices, di_to_hash(&di));
}
return devices;
}
int main(int argc, char *argv[])
{
int opt, sock, dev_id, lap = 0, uap = 0, delay = 5;
int have_lap = 0;
int have_uap = 0;
int afh_enabled = 0;
uint8_t mode, afh_map[10];
char *end, ubertooth_device = -1;
char *bt_dev = "hci0";
char addr[19] = { 0 };
struct libusb_device_handle *devh = NULL;
uint32_t clock;
uint16_t accuracy, handle, offset;
bdaddr_t bdaddr;
btbb_piconet *pn;
struct hci_dev_info di;
int cc = 0;
pn = btbb_piconet_new();
while ((opt=getopt(argc,argv,"hl:u:U:e:d:ab:w:")) != EOF) {
switch(opt) {
case 'l':
lap = strtol(optarg, &end, 16);
if (end != optarg) {
++have_lap;
}
break;
case 'u':
uap = strtol(optarg, &end, 16);
if (end != optarg) {
++have_uap;
}
break;
case 'U':
ubertooth_device = atoi(optarg);
break;
case 'e':
max_ac_errors = atoi(optarg);
break;
case 'd':
dumpfile = fopen(optarg, "w");
if (dumpfile == NULL) {
perror(optarg);
return 1;
}
break;
case 'a':
afh_enabled = 1;
break;
case 'b':
bt_dev = optarg;
if (bt_dev == NULL) {
perror(optarg);
return 1;
}
break;
case 'w': //wait
delay = atoi(optarg);
break;
case 'h':
default:
usage();
return 1;
}
}
dev_id = hci_devid(bt_dev);
sock = hci_open_dev(dev_id);
hci_read_clock(sock, 0, 0, &clock, &accuracy, 0);
if ((have_lap != 1) || (have_uap != 1)) {
printf("No address given, reading address from device\n");
hci_read_bd_addr(sock, &bdaddr, 0);
lap = bdaddr.b[0] | bdaddr.b[1] << 8 | bdaddr.b[2] << 16;
btbb_init_piconet(pn, lap);
uap = bdaddr.b[3];
btbb_piconet_set_uap(pn, uap);
printf("LAP=%06x UAP=%02x\n", lap, uap);
} else if (have_lap && have_uap) {
btbb_init_piconet(pn, lap);
btbb_piconet_set_uap(pn, uap);
printf("Address given, assuming address is remote\n");
sprintf(addr, "00:00:%02X:%02X:%02X:%02X",
uap,
(lap >> 16) & 0xFF,
(lap >> 8) & 0xFF,
lap & 0xFF
);
str2ba(addr, &bdaddr);
printf("Address: %s\n", addr);
if (hci_devinfo(dev_id, &di) < 0) {
perror("Can't get device info");
return 1;
}
if (hci_create_connection(sock, &bdaddr,
htobs(di.pkt_type & ACL_PTYPE_MASK),
0, 0x01, &handle, 25000) < 0) {
perror("Can't create connection");
return 1;
}
sleep(1);
cc = 1;
if (hci_read_clock_offset(sock, handle, &offset, 1000) < 0) {
perror("Reading clock offset failed");
}
clock += offset;
//Experimental AFH map reading from remote device
if(afh_enabled) {
if(hci_read_afh_map(sock, handle, &mode, afh_map, 1000) < 0) {
perror("HCI read AFH map request failed");
//exit(1);
}
if(mode == 0x01) {
btbb_piconet_set_afh_map(pn, afh_map);
btbb_print_afh_map(pn);
} else {
printf("AFH disabled.\n");
afh_enabled = 0;
}
}
if (cc) {
usleep(10000);
hci_disconnect(sock, handle, HCI_OE_USER_ENDED_CONNECTION, 10000);
}
} else {
int main(int argc, char *argv[])
{
int opt, sock, dev_id, lap = 0, uap = 0, delay = 5;
int have_lap = 0;
int have_uap = 0;
int afh_enabled = 0;
uint8_t mode, afh_map[10];
char *end, ubertooth_device = -1;
char *bt_dev = "hci0";
char addr[19] = { 0 };
uint32_t clock;
uint16_t accuracy, handle, offset;
bdaddr_t bdaddr;
btbb_piconet *pn;
struct hci_dev_info di;
int cc = 0;
pn = btbb_piconet_new();
while ((opt=getopt(argc,argv,"hl:u:U:e:d:ab:w:r:q:")) != EOF) {
switch(opt) {
case 'l':
lap = strtol(optarg, &end, 16);
if (end != optarg) {
++have_lap;
}
break;
case 'u':
uap = strtol(optarg, &end, 16);
if (end != optarg) {
++have_uap;
}
break;
case 'U':
ubertooth_device = atoi(optarg);
break;
case 'r':
if (!h_pcapng_bredr) {
if (btbb_pcapng_create_file( optarg, "Ubertooth", &h_pcapng_bredr )) {
err(1, "create_bredr_capture_file: ");
}
}
else {
printf("Ignoring extra capture file: %s\n", optarg);
}
break;
#ifdef ENABLE_PCAP
case 'q':
if (!h_pcap_bredr) {
if (btbb_pcap_create_file(optarg, &h_pcap_bredr)) {
err(1, "btbb_pcap_create_file: ");
}
}
else {
printf("Ignoring extra capture file: %s\n", optarg);
}
break;
#endif
case 'e':
max_ac_errors = atoi(optarg);
break;
case 'd':
dumpfile = fopen(optarg, "w");
if (dumpfile == NULL) {
perror(optarg);
return 1;
}
break;
case 'a':
afh_enabled = 1;
break;
case 'b':
bt_dev = optarg;
if (bt_dev == NULL) {
perror(optarg);
return 1;
}
break;
case 'w': //wait
delay = atoi(optarg);
break;
case 'h':
default:
usage();
return 1;
}
}
dev_id = hci_devid(bt_dev);
sock = hci_open_dev(dev_id);
hci_read_clock(sock, 0, 0, &clock, &accuracy, 0);
if ((have_lap != 1) || (have_uap != 1)) {
printf("No address given, reading address from device\n");
hci_read_bd_addr(sock, &bdaddr, 0);
lap = bdaddr.b[0] | bdaddr.b[1] << 8 | bdaddr.b[2] << 16;
btbb_init_piconet(pn, lap);
uap = bdaddr.b[3];
btbb_piconet_set_uap(pn, uap);
printf("LAP=%06x UAP=%02x\n", lap, uap);
} else if (have_lap && have_uap) {
btbb_init_piconet(pn, lap);
btbb_piconet_set_uap(pn, uap);
printf("Address given, assuming address is remote\n");
sprintf(addr, "00:00:%02X:%02X:%02X:%02X",
uap,
(lap >> 16) & 0xFF,
(lap >> 8) & 0xFF,
lap & 0xFF
);
str2ba(addr, &bdaddr);
printf("Address: %s\n", addr);
if (hci_devinfo(dev_id, &di) < 0) {
perror("Can't get device info");
return 1;
}
if (hci_create_connection(sock, &bdaddr,
htobs(di.pkt_type & ACL_PTYPE_MASK),
0, 0x01, &handle, 25000) < 0) {
perror("Can't create connection");
return 1;
}
sleep(1);
cc = 1;
if (hci_read_clock_offset(sock, handle, &offset, 1000) < 0) {
perror("Reading clock offset failed");
}
clock += offset;
} else {
QString QNetworkInfoPrivate::macAddress(QNetworkInfo::NetworkMode mode, int interface)
{
switch (mode) {
case QNetworkInfo::WlanMode: {
QStringList dirs = QDir(*NETWORK_SYSFS_PATH()).entryList(*WLAN_MASK());
if (interface < dirs.size()) {
QFile carrier(*NETWORK_SYSFS_PATH() + dirs.at(interface) + QString(QStringLiteral("/address")));
if (carrier.open(QIODevice::ReadOnly))
return QString::fromLatin1(carrier.readAll().simplified().data());
}
break;
}
case QNetworkInfo::EthernetMode: {
QStringList dirs = QDir(*NETWORK_SYSFS_PATH()).entryList(*ETHERNET_MASK());
if (interface < dirs.size()) {
QFile carrier(*NETWORK_SYSFS_PATH() + dirs.at(interface) + QString(QStringLiteral("/address")));
if (carrier.open(QIODevice::ReadOnly))
return QString::fromLatin1(carrier.readAll().simplified().data());
}
break;
}
case QNetworkInfo::BluetoothMode: {
#if !defined(QT_NO_BLUEZ)
int ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (ctl < 0)
break;
struct hci_dev_list_req *deviceList = (struct hci_dev_list_req *)malloc(HCI_MAX_DEV * sizeof(struct hci_dev_req) + sizeof(uint16_t));
deviceList->dev_num = HCI_MAX_DEV;
QString macAddress;
if (ioctl(ctl, HCIGETDEVLIST, deviceList) == 0) {
int count = deviceList->dev_num;
if (interface < count) {
struct hci_dev_info deviceInfo;
deviceInfo.dev_id = (deviceList->dev_req + interface)->dev_id;
if (ioctl(ctl, HCIGETDEVINFO, &deviceInfo) == 0) {
// do not use BDADDR_ANY, fails with gcc 4.6
bdaddr_t bdaddr_any = (bdaddr_t) {{0, 0, 0, 0, 0, 0}};
if (hci_test_bit(HCI_RAW, &deviceInfo.flags) && !bacmp(&deviceInfo.bdaddr, &bdaddr_any)) {
int hciDevice = hci_open_dev(deviceInfo.dev_id);
hci_read_bd_addr(hciDevice, &deviceInfo.bdaddr, 1000);
hci_close_dev(hciDevice);
}
char address[18];
ba2str(&deviceInfo.bdaddr, address);
macAddress = QString::fromLatin1(address);
}
}
}
free(deviceList);
close(ctl);
return macAddress;
#else
break;
#endif // QT_NO_BLUEZ
}
// case QNetworkInfo::GsmMode:
// case QNetworkInfo::CdmaMode:
// case QNetworkInfo::WcdmaMode:
// case QNetworkInfo::WimaxMode:
// case QNetworkInfo::LteMode:
// case QNetworkInfo::TdscdmaMode:
default:
break;
};
return QString();
}
int main(int argc, char *argv[])
{
struct sigaction sa;
bdaddr_t bdaddr = { 0 };
int ctl, csk, isk, debug, legacy, remote;
if (argc > 3) {
debug = atoi(argv[1]);
legacy = atoi(argv[2]);
remote = atoi(argv[3]);
} else {
std::cerr << argv[0] << " requires 'sixad'. Please run sixad instead" << std::endl;
return 1;
}
#if 0
// Enable all bluetooth adapters
int hci_ctl;
if ((hci_ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) >= 0) {
for (int i=0; i < 4; i++)
{
di.dev_id = i;
if (ioctl(hci_ctl, HCIGETDEVINFO, (void *) &di) == 0)
{
if (hci_test_bit(HCI_RAW, &di.flags) && !bacmp(&di.bdaddr, BDADDR_ANY)) {
int dd = hci_open_dev(di.dev_id);
hci_read_bd_addr(dd, &di.bdaddr, 1000);
hci_close_dev(dd);
}
}
cmd_reset(hci_ctl, di.dev_id);
}
}
#endif
open_log("sixad-bin");
syslog(LOG_INFO, "started");
ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HIDP);
if (ctl < 0) {
syslog(LOG_ERR, "Can't open HIDP control socket");
close(ctl);
return 1;
}
if (remote) {
// BD Remote only
syslog(LOG_INFO, "BD Remote mode active, hold Enter+Start on your remote now");
while (!io_canceled()) {
do_search(ctl, &bdaddr, debug);
sleep(2);
}
} else {
// Normal behaviour
csk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_CTRL, L2CAP_LM_MASTER, 10);
if (csk < 0) {
syslog(LOG_ERR, "Can't listen on HID control channel");
close(csk);
close(ctl);
return 1;
}
isk = l2cap_listen(&bdaddr, L2CAP_PSM_HIDP_INTR, L2CAP_LM_MASTER, 10);
if (isk < 0) {
syslog(LOG_ERR, "Can't listen on HID interrupt channel");
close(isk);
close(csk);
close(ctl);
return 1;
}
memset(&sa, 0, sizeof(sa));
sa.sa_flags = SA_NOCLDSTOP;
sa.sa_handler = sig_term;
sigaction(SIGTERM, &sa, NULL);
sigaction(SIGINT, &sa, NULL);
sa.sa_handler = sig_hup;
sigaction(SIGHUP, &sa, NULL);
sa.sa_handler = SIG_IGN;
sigaction(SIGCHLD, &sa, NULL);
sigaction(SIGPIPE, &sa, NULL);
syslog(LOG_INFO, "sixad started, press the PS button now");
hid_server(ctl, csk, isk, debug, legacy);
close(isk);
close(csk);
}
close(ctl);
syslog(LOG_INFO, "Done");
return 0;
}
/**
* @brief Open Socket
*
* @param dev Device handle
*
* @return
*/
static int open_socket(int dev)
{
struct sockaddr_hci addr;
struct hci_filter flt;
struct hci_dev_info di;
int sk, opt;
if (dev != HCI_DEV_NONE) {
int dd = hci_open_dev(dev);
if (dd < 0) {
perror("Can't open device");
return -1;
}
if (hci_devinfo(dev, &di) < 0) {
perror("Can't get device info");
return -1;
}
if (hci_read_bd_addr(dd, &g_mac_address, 1000) < 0 ) {
perror("Can't get device mac address");
return -1;
}
opt = hci_test_bit(HCI_RAW, &di.flags);
if (ioctl(dd, HCISETRAW, opt) < 0) {
if (errno == EACCES) {
perror("Can't access device");
return -1;
}
}
hci_close_dev(dd);
}
/* Create HCI socket */
sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (sk < 0) {
perror("Can't create raw socket");
return -1;
}
opt = 1;
if (setsockopt(sk, SOL_HCI, HCI_DATA_DIR, &opt, sizeof(opt)) < 0) {
perror("Can't enable data direction info");
return -1;
}
opt = 1;
if (setsockopt(sk, SOL_HCI, HCI_TIME_STAMP, &opt, sizeof(opt)) < 0) {
perror("Can't enable time stamp");
return -1;
}
/* Setup filter */
hci_filter_clear(&flt);
hci_filter_all_ptypes(&flt);
hci_filter_all_events(&flt);
if (setsockopt(sk, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
perror("Can't set filter");
return -1;
}
/* Bind socket to the HCI device */
memset(&addr, 0, sizeof(addr));
addr.hci_family = AF_BLUETOOTH;
addr.hci_dev = dev;
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
fprintf(stderr, "Can't attach to device hci%d. %s(%d)\n",
dev, strerror(errno), errno);
return -1;
}
return sk;
}
int main(int argc, char *argv[])
{
struct sigaction sa;
struct hci_dev_info di;
struct hci_version ver;
struct hci_filter flt;
bdaddr_t bdaddr, master, slave;
int need_raw;
int dd, opt, dev = 0;
bacpy(&slave, BDADDR_ANY);
while ((opt=getopt_long(argc, argv, "+i:h", main_options, NULL)) != -1) {
switch (opt) {
case 'i':
dev = hci_devid(optarg);
if (dev < 0) {
perror("Invalid device");
exit(1);
}
break;
case 'h':
default:
usage();
exit(0);
}
}
argc -= optind;
argv += optind;
optind = 0;
if (argc < 1) {
usage();
exit(1);
}
str2ba(argv[0], &master);
if (argc > 1)
str2ba(argv[1], &slave);
dd = hci_open_dev(dev);
if (dd < 0) {
fprintf(stderr, "Can't open device hci%d: %s (%d)\n",
dev, strerror(errno), errno);
exit(1);
}
if (hci_devinfo(dev, &di) < 0) {
fprintf(stderr, "Can't get device info for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
if (hci_read_local_version(dd, &ver, 1000) < 0) {
fprintf(stderr, "Can't read version for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
if (ver.manufacturer != 10 || id2ver(ver.hci_rev) < 0) {
fprintf(stderr, "Can't find sniffer at hci%d: %s (%d)\n",
dev, strerror(ENOSYS), ENOSYS);
hci_close_dev(dd);
exit(1);
}
if (!bacmp(&di.bdaddr, BDADDR_ANY)) {
if (hci_read_bd_addr(dd, &bdaddr, 1000) < 0) {
fprintf(stderr, "Can't read address for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
} else
bacpy(&bdaddr, &di.bdaddr);
need_raw = !hci_test_bit(HCI_RAW, &di.flags);
hci_filter_clear(&flt);
hci_filter_set_ptype(HCI_ACLDATA_PKT, &flt);
hci_filter_set_ptype(HCI_EVENT_PKT, &flt);
hci_filter_set_event(EVT_VENDOR, &flt);
if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
fprintf(stderr, "Can't set filter for hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
memset(&sa, 0, sizeof(sa));
sa.sa_flags = SA_NOCLDSTOP;
sa.sa_handler = SIG_IGN;
sigaction(SIGCHLD, &sa, NULL);
sigaction(SIGPIPE, &sa, NULL);
sa.sa_handler = sig_term;
sigaction(SIGTERM, &sa, NULL);
sigaction(SIGINT, &sa, NULL);
sa.sa_handler = sig_hup;
sigaction(SIGHUP, &sa, NULL);
if (need_raw) {
if (ioctl(dd, HCISETRAW, 1) < 0) {
fprintf(stderr, "Can't set raw mode on hci%d: %s (%d)\n",
dev, strerror(errno), errno);
hci_close_dev(dd);
exit(1);
}
}
printf("CSR sniffer - Bluetooth packet analyzer ver %s\n", VERSION);
if (need_raw) {
if (ioctl(dd, HCISETRAW, 0) < 0)
fprintf(stderr, "Can't clear raw mode on hci%d: %s (%d)\n",
dev, strerror(errno), errno);
}
hci_close_dev(dd);
return 0;
}
int main(int argc, const char* argv[]) {
const char *hciDeviceIdOverride = NULL;
int hciDeviceId = 0;
int hciSocket;
int serverL2capSock;
struct sockaddr_l2 sockAddr;
socklen_t sockAddrLen;
int result;
bdaddr_t clientBdAddr;
int clientL2capSock;
struct l2cap_conninfo l2capConnInfo;
socklen_t l2capConnInfoLen;
int hciHandle;
fd_set afds;
fd_set rfds;
struct timeval tv;
char stdinBuf[256 * 2 + 1];
char l2capSockBuf[256];
int len;
int i;
struct bt_security btSecurity;
socklen_t btSecurityLen;
uint8_t securityLevel = 0;
// remove buffering
setbuf(stdin, NULL);
setbuf(stdout, NULL);
setbuf(stderr, NULL);
// setup signal handlers
signal(SIGINT, signalHandler);
signal(SIGKILL, signalHandler);
signal(SIGHUP, signalHandler);
signal(SIGUSR1, signalHandler);
prctl(PR_SET_PDEATHSIG, SIGINT);
// create socket
serverL2capSock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (argc > 1 && strlen(argv[1]) > 0) {
hciDeviceIdOverride = argv[1];
}
if (hciDeviceIdOverride != NULL) {
hciDeviceId = atoi(hciDeviceIdOverride);
} else {
// if no env variable given, use the first available device
hciDeviceId = hci_get_route(NULL);
}
if (hciDeviceId < 0) {
hciDeviceId = 0; // use device 0, if device id is invalid
}
printf("hciDeviceId %d\n", hciDeviceId);
bdaddr_t daddr;
hciSocket = hci_open_dev(hciDeviceId);
if (hciSocket == -1) {
printf("adapterState unsupported\n");
return -1;
}
if (hci_read_bd_addr(hciSocket, &daddr, 1000) == -1){
daddr = *BDADDR_ANY;
}
printf("bdaddr ");
for(i = 5; i > 0; i--) {
printf("%02x:", daddr.b[i]);
}
printf("%02x", daddr.b[0]);
printf("\n");
// bind
memset(&sockAddr, 0, sizeof(sockAddr));
sockAddr.l2_family = AF_BLUETOOTH;
sockAddr.l2_bdaddr = daddr;
sockAddr.l2_cid = htobs(ATT_CID);
result = bind(serverL2capSock, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
printf("bind %s\n", (result == -1) ? strerror(errno) : "success");
result = listen(serverL2capSock, 1);
printf("listen %s\n", (result == -1) ? strerror(errno) : "success");
while (result != -1) {
FD_ZERO(&afds);
FD_SET(serverL2capSock, &afds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(serverL2capSock + 1, &afds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
} else if (SIGHUP == lastSignal || SIGUSR1 == lastSignal) {
result = 0;
}
} else if (result && FD_ISSET(serverL2capSock, &afds)) {
sockAddrLen = sizeof(sockAddr);
clientL2capSock = accept(serverL2capSock, (struct sockaddr *)&sockAddr, &sockAddrLen);
baswap(&clientBdAddr, &sockAddr.l2_bdaddr);
printf("accept %s\n", batostr(&clientBdAddr));
l2capConnInfoLen = sizeof(l2capConnInfo);
getsockopt(clientL2capSock, SOL_L2CAP, L2CAP_CONNINFO, &l2capConnInfo, &l2capConnInfoLen);
hciHandle = l2capConnInfo.hci_handle;
while(1) {
FD_ZERO(&rfds);
FD_SET(0, &rfds);
FD_SET(clientL2capSock, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(clientL2capSock + 1, &rfds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
} else if (SIGHUP == lastSignal) {
result = 0;
hci_disconnect(hciSocket, hciHandle, HCI_OE_USER_ENDED_CONNECTION, 1000);
} else if (SIGUSR1 == lastSignal) {
int8_t rssi = 0;
for (i = 0; i < 100; i++) {
hci_read_rssi(hciSocket, hciHandle, &rssi, 1000);
if (rssi != 0) {
break;
}
}
if (rssi == 0) {
rssi = 127;
}
printf("rssi = %d\n", rssi);
}
} else if (result) {
if (FD_ISSET(0, &rfds)) {
len = read(0, stdinBuf, sizeof(stdinBuf));
if (len <= 0) {
break;
}
i = 0;
while(stdinBuf[i] != '\n') {
unsigned int data = 0;
sscanf(&stdinBuf[i], "%02x", &data);
l2capSockBuf[i / 2] = data;
i += 2;
}
len = write(clientL2capSock, l2capSockBuf, (len - 1) / 2);
}
if (FD_ISSET(clientL2capSock, &rfds)) {
len = read(clientL2capSock, l2capSockBuf, sizeof(l2capSockBuf));
if (len <= 0) {
break;
}
btSecurityLen = sizeof(btSecurity);
memset(&btSecurity, 0, btSecurityLen);
getsockopt(clientL2capSock, SOL_BLUETOOTH, BT_SECURITY, &btSecurity, &btSecurityLen);
if (securityLevel != btSecurity.level) {
securityLevel = btSecurity.level;
const char *securityLevelString;
switch(securityLevel) {
case BT_SECURITY_LOW:
securityLevelString = "low";
break;
case BT_SECURITY_MEDIUM:
securityLevelString = "medium";
break;
case BT_SECURITY_HIGH:
securityLevelString = "high";
break;
default:
securityLevelString = "unknown";
break;
}
printf("security %s\n", securityLevelString);
}
printf("data ");
for(i = 0; i < len; i++) {
printf("%02x", ((int)l2capSockBuf[i]) & 0xff);
}
printf("\n");
}
}
}
printf("disconnect %s\n", batostr(&clientBdAddr));
close(clientL2capSock);
}
}
printf("close\n");
close(serverL2capSock);
close(hciSocket);
return 0;
}