void up_hci(int hci_idx)
{
int sk, i;
struct hci_dev_info hci_info;
sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (sk < 0)
{
perror("Fail to create bluetooth hci socket");
return;
}
memset(&hci_info, 0, sizeof(hci_info));
hci_info.dev_id = hci_idx;
for (i = 0; i < MAX_RETRY; i++)
{
if (ioctl(sk, HCIGETDEVINFO, (void *) &hci_info) < 0)
{
perror("Failed to get HCI device information");
/* sleep 100ms */
usleep(100*1000);
continue;
}
if (hci_test_bit(HCI_RUNNING, &hci_info.flags) && !hci_test_bit(HCI_INIT, &hci_info.flags))
{
/* check if kernel has already set device UP... */
if (!hci_test_bit(HCI_UP, &hci_info.flags))
{
if (ioctl(sk, HCIDEVUP, hci_idx) < 0)
{
/* ignore if device is already UP and ready */
if (errno == EALREADY)
break;
perror("Fail to set hci device UP");
}
}
break;
}
/* sleep 100ms */
usleep(100*1000);
}
close(sk);
}
/* Send frame to RAW socket */
void hci_send_to_sock(struct hci_dev *hdev, struct sk_buff *skb)
{
struct sock * sk;
BT_DBG("hdev %p len %d", hdev, skb->len);
read_lock(&hci_sk_list.lock);
for (sk = hci_sk_list.head; sk; sk = sk->next) {
struct hci_filter *flt;
struct sk_buff *nskb;
if (sk->state != BT_BOUND || hci_pi(sk)->hdev != hdev)
continue;
/* Don't send frame to the socket it came from */
if (skb->sk == sk)
continue;
/* Apply filter */
flt = &hci_pi(sk)->filter;
if (!hci_test_bit((skb->pkt_type & HCI_FLT_TYPE_BITS), &flt->type_mask))
continue;
if (skb->pkt_type == HCI_EVENT_PKT) {
register int evt = (*(__u8 *)skb->data & HCI_FLT_EVENT_BITS);
if (!hci_test_bit(evt, &flt->event_mask))
continue;
if (flt->opcode && ((evt == EVT_CMD_COMPLETE &&
flt->opcode != *(__u16 *)(skb->data + 3)) ||
(evt == EVT_CMD_STATUS &&
flt->opcode != *(__u16 *)(skb->data + 4))))
continue;
}
if (!(nskb = skb_clone(skb, GFP_ATOMIC)))
continue;
/* Put type byte before the data */
memcpy(skb_push(nskb, 1), &nskb->pkt_type, 1);
if (sock_queue_rcv_skb(sk, nskb))
kfree_skb(nskb);
}
read_unlock(&hci_sk_list.lock);
}
int bt_is_enabled() {
LOGV(__FUNCTION__);
int hci_sock = -1;
int ret = -1;
struct hci_dev_info dev_info;
// Check power first
ret = check_bluetooth_power();
if (ret == -1 || ret == 0) goto out;
ret = -1;
// Power is on, now check if the HCI interface is up
hci_sock = create_hci_sock();
if (hci_sock < 0) goto out;
dev_info.dev_id = HCI_DEV_ID;
if (ioctl(hci_sock, HCIGETDEVINFO, (void *)&dev_info) < 0) {
ret = 0;
goto out;
}
ret = hci_test_bit(HCI_UP, &dev_info.flags);
out:
if (hci_sock >= 0) close(hci_sock);
return ret;
}
static int init_known_adapters(int ctl)
{
struct hci_dev_list_req *dl;
struct hci_dev_req *dr;
int i, err;
dl = g_try_malloc0(HCI_MAX_DEV * sizeof(struct hci_dev_req) + sizeof(uint16_t));
if (!dl) {
err = errno;
error("Can't allocate devlist buffer: %s (%d)",
strerror(err), err);
return -err;
}
dl->dev_num = HCI_MAX_DEV;
dr = dl->dev_req;
if (ioctl(ctl, HCIGETDEVLIST, (void *) dl) < 0) {
err = errno;
error("Can't get device list: %s (%d)",
strerror(err), err);
g_free(dl);
return -err;
}
for (i = 0; i < dl->dev_num; i++, dr++) {
device_event(HCI_DEV_REG, dr->dev_id);
if (hci_test_bit(HCI_UP, &dr->dev_opt))
device_event(HCI_DEV_UP, dr->dev_id);
}
g_free(dl);
return 0;
}
bool CBlueZStack::IsEnabled()
{
struct hci_dev_info di;
if (hci_devinfo(g_devId, &di) < 0) {
return false;
}
return hci_test_bit(HCI_UP, &(di.flags));
}
static void device_devreg_setup(int index)
{
struct hci_dev_info di;
gboolean devup;
init_device(index);
memset(&di, 0, sizeof(di));
if (hci_devinfo(index, &di) < 0)
return;
devup = hci_test_bit(HCI_UP, &di.flags);
if (!hci_test_bit(HCI_RAW, &di.flags))
manager_register_adapter(index, devup);
}
void update_service_classes(const bdaddr_t *bdaddr, uint8_t value)
{
struct hci_dev_list_req *dl;
struct hci_dev_req *dr;
int i, sk;
sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (sk < 0)
return;
dl = g_malloc0(HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl));
dl->dev_num = HCI_MAX_DEV;
dr = dl->dev_req;
if (ioctl(sk, HCIGETDEVLIST, dl) < 0) {
close(sk);
g_free(dl);
return;
}
dr = dl->dev_req;
for (i = 0; i < dl->dev_num; i++, dr++) {
struct hci_dev_info di;
uint8_t cls[3];
int dd;
if (hci_devinfo(dr->dev_id, &di) < 0)
continue;
if (hci_test_bit(HCI_RAW, &di.flags))
continue;
if (get_device_class(di.dev_id, cls) < 0)
continue;
dd = hci_open_dev(di.dev_id);
if (dd < 0)
continue;
set_service_classes(dd, cls, value);
hci_close_dev(dd);
update_adapter(di.dev_id);
}
g_free(dl);
close(sk);
}
VALUE di_to_hash(struct hci_dev_info *di)
{
VALUE hash = rb_hash_new();
rb_hash_aset(hash, ID2SYM(rb_intern("device_id")), INT2FIX(di->dev_id));
rb_hash_aset(hash, ID2SYM(rb_intern("name")), rb_str_new2(di->name));
rb_hash_aset(hash, ID2SYM(rb_intern("addr")), ba2value(&di->bdaddr));
if (hci_test_bit(HCI_UP, &di->flags)) {
rb_hash_aset(hash, ID2SYM(rb_intern("up")), Qtrue);
} else {
rb_hash_aset(hash, ID2SYM(rb_intern("up")), Qfalse);
}
return hash;
}
static DBusMessage *find_adapter(DBusConnection *conn,
DBusMessage *msg, void *data)
{
DBusMessage *reply;
struct adapter *adapter;
char *path;
struct hci_dev_info di;
const char *pattern;
int dev_id;
if (!dbus_message_get_args(msg, NULL, DBUS_TYPE_STRING, &pattern,
DBUS_TYPE_INVALID))
return NULL;
/* hci_devid() would make sense to use here, except it
is restricted to devices which are up */
if (!strncmp(pattern, "hci", 3) && strlen(pattern) >= 4)
dev_id = atoi(pattern + 3);
else
dev_id = find_by_address(pattern);
if (dev_id < 0)
return no_such_adapter(msg);
if (hci_devinfo(dev_id, &di) < 0)
return no_such_adapter(msg);
if (hci_test_bit(HCI_RAW, &di.flags))
return no_such_adapter(msg);
adapter = manager_find_adapter_by_id(dev_id);
if (!adapter)
return no_such_adapter(msg);
reply = dbus_message_new_method_return(msg);
if (!reply)
return NULL;
path = adapter->path + ADAPTER_PATH_INDEX;
dbus_message_append_args(reply, DBUS_TYPE_OBJECT_PATH, &path,
DBUS_TYPE_INVALID);
return reply;
}
static int find_by_address(const char *str)
{
struct hci_dev_list_req *dl;
struct hci_dev_req *dr;
bdaddr_t ba;
int i, sk;
int devid = -1;
sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (sk < 0)
return -1;
dl = g_malloc0(HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl));
dl->dev_num = HCI_MAX_DEV;
dr = dl->dev_req;
if (ioctl(sk, HCIGETDEVLIST, dl) < 0)
goto out;
dr = dl->dev_req;
str2ba(str, &ba);
for (i = 0; i < dl->dev_num; i++, dr++) {
struct hci_dev_info di;
if (hci_devinfo(dr->dev_id, &di) < 0)
continue;
if (hci_test_bit(HCI_RAW, &di.flags))
continue;
if (!bacmp(&ba, &di.bdaddr)) {
devid = dr->dev_id;
break;
}
}
out:
g_free(dl);
close(sk);
return devid;
}
static void configure_device(int index)
{
struct hci_dev_info di;
uint16_t policy;
int dd;
if (hci_devinfo(index, &di) < 0)
return;
if (hci_test_bit(HCI_RAW, &di.flags))
return;
dd = hci_open_dev(index);
if (dd < 0) {
error("Can't open device hci%d: %s (%d)",
index, strerror(errno), errno);
return;
}
/* Set device name */
if ((main_opts.flags & (1 << HCID_SET_NAME)) && main_opts.name) {
change_local_name_cp cp;
memset(cp.name, 0, sizeof(cp.name));
expand_name((char *) cp.name, sizeof(cp.name),
main_opts.name, index);
hci_send_cmd(dd, OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME,
CHANGE_LOCAL_NAME_CP_SIZE, &cp);
}
/* Set device class */
if ((main_opts.flags & (1 << HCID_SET_CLASS))) {
write_class_of_dev_cp cp;
uint32_t class;
uint8_t cls[3];
if (read_local_class(&di.bdaddr, cls) < 0) {
class = htobl(main_opts.class);
cls[2] = get_service_classes(&di.bdaddr);
memcpy(cp.dev_class, &class, 3);
} else {
if (!(main_opts.scan & SCAN_INQUIRY))
static void device_devup_setup(int index)
{
struct hci_dev_info di;
uint16_t policy;
int dd, err;
if (hci_devinfo(index, &di) < 0)
return;
if (hci_test_bit(HCI_RAW, &di.flags))
return;
dd = hci_open_dev(index);
if (dd < 0) {
err = errno;
error("Can't open device hci%d: %s (%d)",
index, strerror(err), err);
return;
}
/* Set page timeout */
if ((main_opts.flags & (1 << HCID_SET_PAGETO))) {
write_page_timeout_cp cp;
cp.timeout = htobs(main_opts.pageto);
hci_send_cmd(dd, OGF_HOST_CTL, OCF_WRITE_PAGE_TIMEOUT,
WRITE_PAGE_TIMEOUT_CP_SIZE, &cp);
}
/* Set default link policy */
policy = htobs(main_opts.link_policy);
hci_send_cmd(dd, OGF_LINK_POLICY,
OCF_WRITE_DEFAULT_LINK_POLICY, 2, &policy);
hci_close_dev(dd);
start_security_manager(index);
/* Return value 1 means ioctl(DEVDOWN) was performed */
if (manager_start_adapter(index) == 1)
stop_security_manager(index);
}
static DBusMessage *old_find_adapter(DBusConnection *conn,
DBusMessage *msg, void *data)
{
DBusMessage *reply;
char path[MAX_PATH_LENGTH], *path_ptr = path;
struct hci_dev_info di;
const char *pattern;
int dev_id;
if (!dbus_message_get_args(msg, NULL,
DBUS_TYPE_STRING, &pattern,
DBUS_TYPE_INVALID))
return invalid_args(msg);
/* hci_devid() would make sense to use here, except it
is restricted to devices which are up */
if (!strncmp(pattern, "hci", 3) && strlen(pattern) >= 4)
dev_id = atoi(pattern + 3);
else
dev_id = find_by_address(pattern);
if (dev_id < 0)
return no_such_adapter(msg);
if (hci_devinfo(dev_id, &di) < 0)
return no_such_adapter(msg);
if (hci_test_bit(HCI_RAW, &di.flags))
return no_such_adapter(msg);
reply = dbus_message_new_method_return(msg);
if (!reply)
return NULL;
snprintf(path, sizeof(path), "%s/hci%d", BASE_PATH, dev_id);
dbus_message_append_args(reply, DBUS_TYPE_STRING, &path_ptr,
DBUS_TYPE_INVALID);
return reply;
}
static DBusMessage *list_adapters(DBusConnection *conn,
DBusMessage *msg, void *data)
{
DBusMessageIter iter;
DBusMessageIter array_iter;
DBusMessage *reply;
GSList *l;
reply = dbus_message_new_method_return(msg);
if (!reply)
return NULL;
dbus_message_iter_init_append(reply, &iter);
dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
DBUS_TYPE_OBJECT_PATH_AS_STRING, &array_iter);
for (l = adapters; l; l = l->next) {
struct adapter *adapter = l->data;
char *path;
struct hci_dev_info di;
if (hci_devinfo(adapter->dev_id, &di) < 0)
continue;
if (hci_test_bit(HCI_RAW, &di.flags))
continue;
path = adapter->path + ADAPTER_PATH_INDEX;
dbus_message_iter_append_basic(&array_iter,
DBUS_TYPE_OBJECT_PATH, &path);
}
dbus_message_iter_close_container(&iter, &array_iter);
return reply;
}
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;
}
static void configure_device(int index)
{
struct hci_dev_info di;
uint16_t policy;
int dd, err;
if (hci_devinfo(index, &di) < 0)
return;
if (hci_test_bit(HCI_RAW, &di.flags))
return;
dd = hci_open_dev(index);
if (dd < 0) {
err = errno;
error("Can't open device hci%d: %s (%d)",
index, strerror(err), err);
return;
}
/* Set page timeout */
if ((main_opts.flags & (1 << HCID_SET_PAGETO))) {
write_page_timeout_cp cp;
cp.timeout = htobs(main_opts.pageto);
hci_send_cmd(dd, OGF_HOST_CTL, OCF_WRITE_PAGE_TIMEOUT,
WRITE_PAGE_TIMEOUT_CP_SIZE, &cp);
}
/* Set default link policy */
policy = htobs(main_opts.link_policy);
hci_send_cmd(dd, OGF_LINK_POLICY,
OCF_WRITE_DEFAULT_LINK_POLICY, 2, &policy);
hci_close_dev(dd);
}
int
brcm_hci_for_each_dev(int flag, int (*func)(int s, int dev_id, void *context), void *context)
{
int dev_id = -1;
if (!func)
return -1;
int s;
if ((s = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) < 0)
return -1;
struct hci_dev_req *dr;
struct hci_dev_list_req *dl = NULL;
if ((dl = malloc(HCI_MAX_DEV * sizeof (*dr) + sizeof(*dl))) == NULL)
goto done;
dl->dev_num = HCI_MAX_DEV;
dr = dl->dev_req;
if (ioctl(s, HCIGETDEVLIST, (void *)dl))
goto done;
for (int i = 0; i < dl->dev_num; i++, dr++) {
if (hci_test_bit(flag, &dr->dev_opt))
if (!func || func(s, dr->dev_id, context)) {
dev_id = dr->dev_id;
break;
}
}
done:
close(s);
free(dl);
return dev_id;
}
int
do_configure_device(int hdev)
{
struct device_opts *device_opts;
struct hci_dev_req dr;
struct hci_dev_info di;
int s;
set_title("hci%d config", hdev);
if ((s = hci_open_dev(hdev)) < 0) {
syslog(LOG_ERR, "Can't open device hci%d: %s (%d)",
hdev, strerror(errno), errno);
exit(1);
}
di.dev_id = hdev;
if (ioctl(s, HCIGETDEVINFO, (void *) &di) < 0)
exit(1);
if (hci_test_bit(HCI_RAW, &di.flags))
exit(0);
dr.dev_id = hdev;
device_opts = get_device_opts(s, hdev);
/* Set scan mode */
dr.dev_opt = device_opts->scan;
if (ioctl(s, HCISETSCAN, (unsigned long) &dr) < 0) {
syslog(LOG_ERR, "Can't set scan mode on hci%d: %s (%d)",
hdev, strerror(errno), errno);
}
/* Set authentication */
if (device_opts->auth)
dr.dev_opt = AUTH_ENABLED;
else
dr.dev_opt = AUTH_DISABLED;
if (ioctl(s, HCISETAUTH, (unsigned long) &dr) < 0) {
syslog(LOG_ERR, "Can't set auth on hci%d: %s (%d)",
hdev, strerror(errno), errno);
}
/* Set encryption */
if (device_opts->encrypt)
dr.dev_opt = ENCRYPT_P2P;
else
dr.dev_opt = ENCRYPT_DISABLED;
if (ioctl(s, HCISETENCRYPT, (unsigned long) &dr) < 0) {
syslog(LOG_ERR, "Can't set encrypt on hci%d: %s (%d)",
hdev, strerror(errno), errno);
}
/* Set device name */
if ((device_opts->flags & (1 << HCID_SET_NAME)) && device_opts->name) {
change_local_name_cp cp;
write_ext_inquiry_response_cp ip;
uint8_t len;
memset(cp.name, 0, sizeof(cp.name));
expand_name((char *) cp.name, sizeof(cp.name), device_opts->name, hdev);
ip.fec = 0x00;
memset(ip.data, 0, sizeof(ip.data));
len = strlen((char *) cp.name);
if (len > 48) {
len = 48;
ip.data[1] = 0x08;
} else
ip.data[1] = 0x09;
ip.data[0] = len + 1;
memcpy(ip.data + 2, cp.name, len);
hci_send_cmd(s, OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME,
CHANGE_LOCAL_NAME_CP_SIZE, &cp);
if (di.features[6] & LMP_EXT_INQ)
hci_send_cmd(s, OGF_HOST_CTL, OCF_WRITE_EXT_INQUIRY_RESPONSE,
WRITE_EXT_INQUIRY_RESPONSE_CP_SIZE, &ip);
}
/* Set device class */
if ((device_opts->flags & (1 << HCID_SET_CLASS))) {
uint32_t class = htobl(device_opts->class);
write_class_of_dev_cp cp;
memcpy(cp.dev_class, &class, 3);
hci_send_cmd(s, OGF_HOST_CTL, OCF_WRITE_CLASS_OF_DEV,
WRITE_CLASS_OF_DEV_CP_SIZE, &cp);
}
int main(int argc, const char* argv[])
{
char *hciDeviceIdOverride = NULL;
int hciDeviceId = 0;
int hciSocket;
struct hci_dev_info hciDevInfo;
char address[18];
int previousAdapterState = -1;
int currentAdapterState;
const char* adapterState = NULL;
fd_set rfds;
struct timeval tv;
int selectRetval;
char stdinBuf[256 * 2 + 1];
char advertisementDataBuf[256];
int advertisementDataLen = 0;
char scanDataBuf[256];
int scanDataLen = 0;
int len;
int i;
memset(&hciDevInfo, 0x00, sizeof(hciDevInfo));
// 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, SIGKILL);
hciDeviceIdOverride = getenv("BLENO_HCI_DEVICE_ID");
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
}
// setup HCI socket
hciSocket = hci_open_dev(hciDeviceId);
hciDevInfo.dev_id = hciDeviceId;
if (hciSocket == -1) {
printf("adapterState unsupported\n");
return -1;
}
while(1) {
FD_ZERO(&rfds);
FD_SET(0, &rfds);
FD_SET(hciSocket, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
// get HCI dev info for adapter state
ioctl(hciSocket, HCIGETDEVINFO, (void *)&hciDevInfo);
currentAdapterState = hci_test_bit(HCI_UP, &hciDevInfo.flags);
if (previousAdapterState != currentAdapterState) {
previousAdapterState = currentAdapterState;
if (!currentAdapterState) {
adapterState = "poweredOff";
} else {
hci_le_set_advertise_enable(hciSocket, 0, 1000);
hci_le_set_advertise_enable(hciSocket, 1, 1000);
if (hci_le_set_advertise_enable(hciSocket, 0, 1000) == -1) {
if (EPERM == errno) {
adapterState = "unauthorized";
} else if (EIO == errno) {
adapterState = "unsupported";
} else {
printf("%d\n", errno);
adapterState = "unknown";
}
} else {
adapterState = "poweredOn";
}
}
ba2str(&hciDevInfo.bdaddr, address);
printf("address %s\n", address);
printf("adapterState %s\n", adapterState);
}
selectRetval = select(hciSocket + 1, &rfds, NULL, NULL, &tv);
if (-1 == selectRetval) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
// done
break;
} else if (SIGHUP == lastSignal) {
// stop advertising
hci_le_set_advertise_enable(hciSocket, 0, 1000);
} else if (SIGUSR1 == lastSignal) {
// stop advertising
hci_le_set_advertise_enable(hciSocket, 0, 1000);
// set scan data
hci_le_set_scan_response_data(hciSocket, (uint8_t*)&scanDataBuf, scanDataLen, 1000);
// set advertisement data
hci_le_set_advertising_data(hciSocket, (uint8_t*)&advertisementDataBuf, advertisementDataLen, 1000);
// set advertisement parameters, mostly to set the advertising interval to 100ms
hci_le_set_advertising_parameters(hciSocket, 1000);
// start advertising
hci_le_set_advertise_enable(hciSocket, 1, 1000);
// set scan data
hci_le_set_scan_response_data(hciSocket, (uint8_t*)&scanDataBuf, scanDataLen, 1000);
// set advertisement data
hci_le_set_advertising_data(hciSocket, (uint8_t*)&advertisementDataBuf, advertisementDataLen, 1000);
}
} else if (selectRetval) {
if (FD_ISSET(0, &rfds)) {
len = readLine(0, stdinBuf, sizeof(stdinBuf));
if (len <= 0) {
break;
}
i = 0;
advertisementDataLen = 0;
while(i < len && stdinBuf[i] != ' ') {
unsigned int data = 0;
sscanf(&stdinBuf[i], "%02x", &data);
advertisementDataBuf[advertisementDataLen] = data;
advertisementDataLen++;
i += 2;
}
i++;
scanDataLen = 0;
while(i < len && stdinBuf[i] != '\n') {
unsigned int data = 0;
sscanf(&stdinBuf[i], "%02x", &data);
scanDataBuf[scanDataLen] = data;
scanDataLen++;
i += 2;
}
// stop advertising
hci_le_set_advertise_enable(hciSocket, 0, 1000);
// set scan data
hci_le_set_scan_response_data(hciSocket, (uint8_t*)&scanDataBuf, scanDataLen, 1000);
// set advertisement data
hci_le_set_advertising_data(hciSocket, (uint8_t*)&advertisementDataBuf, advertisementDataLen, 1000);
// set advertisement parameters, mostly to set the advertising interval to 100ms
hci_le_set_advertising_parameters(hciSocket, 1000);
// start advertising
hci_le_set_advertise_enable(hciSocket, 1, 1000);
// set scan data
hci_le_set_scan_response_data(hciSocket, (uint8_t*)&scanDataBuf, scanDataLen, 1000);
// set advertisement data
hci_le_set_advertising_data(hciSocket, (uint8_t*)&advertisementDataBuf, advertisementDataLen, 1000);
}
}
}
// stop advertising
hci_le_set_advertise_enable(hciSocket, 0, 1000);
close(hciSocket);
return 0;
}
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, const char* argv[])
{
char *hciDeviceIdOverride = NULL;
int hciDeviceId = 0;
int hciSocket;
struct hci_dev_info hciDevInfo;
struct hci_filter oldHciFilter;
struct hci_filter newHciFilter;
socklen_t oldHciFilterLen;
int previousAdapterState = -1;
int currentAdapterState;
const char* adapterState = NULL;
fd_set rfds;
struct timeval tv;
int selectRetval;
unsigned char hciEventBuf[HCI_MAX_EVENT_SIZE];
int hciEventLen;
evt_le_meta_event *leMetaEvent;
le_advertising_info *leAdvertisingInfo;
char btAddress[18];
int i;
int scanning = 0;
int8_t rssi;
memset(&hciDevInfo, 0x00, sizeof(hciDevInfo));
// setup signal handlers
signal(SIGINT, signalHandler);
signal(SIGKILL, signalHandler);
signal(SIGUSR1, signalHandler);
signal(SIGUSR2, signalHandler);
signal(SIGHUP, signalHandler);
prctl(PR_SET_PDEATHSIG, SIGKILL);
// remove buffering
setbuf(stdin, NULL);
setbuf(stdout, NULL);
setbuf(stderr, NULL);
hciDeviceIdOverride = getenv("NOBLE_HCI_DEVICE_ID");
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
}
// setup HCI socket
hciSocket = hci_open_dev(hciDeviceId);
if (hciSocket == -1) {
printf("adapterState unsupported\n");
return -1;
}
hciDevInfo.dev_id = hciDeviceId;
// get old HCI filter
oldHciFilterLen = sizeof(oldHciFilter);
getsockopt(hciSocket, SOL_HCI, HCI_FILTER, &oldHciFilter, &oldHciFilterLen);
// setup new HCI filter
hci_filter_clear(&newHciFilter);
hci_filter_set_ptype(HCI_EVENT_PKT, &newHciFilter);
hci_filter_set_event(EVT_LE_META_EVENT, &newHciFilter);
setsockopt(hciSocket, SOL_HCI, HCI_FILTER, &newHciFilter, sizeof(newHciFilter));
while(1) {
FD_ZERO(&rfds);
FD_SET(hciSocket, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
// get HCI dev info for adapter state
ioctl(hciSocket, HCIGETDEVINFO, (void *)&hciDevInfo);
currentAdapterState = hci_test_bit(HCI_UP, &hciDevInfo.flags);
if (previousAdapterState != currentAdapterState) {
previousAdapterState = currentAdapterState;
if (!currentAdapterState) {
adapterState = "poweredOff";
} else if (hci_le_set_scan_parameters(hciSocket, 0x01, htobs(0x0010), htobs(0x0010), 0x00, 0, 1000) < 0) {
if (EPERM == errno) {
adapterState = "unauthorized";
} else if (EIO == errno) {
adapterState = "unsupported";
} else {
adapterState = "unknown";
}
} else {
adapterState = "poweredOn";
}
printf("adapterState %s\n", adapterState);
}
selectRetval = select(hciSocket + 1, &rfds, NULL, NULL, &tv);
if (-1 == selectRetval) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
// done
break;
} else if (SIGUSR1 == lastSignal) {
// start scan, filter
scanning = 1;
hci_le_set_scan_enable(hciSocket, 0x00, 1, 1000);
hci_le_set_scan_enable(hciSocket, 0x01, 1, 1000);
} else if (SIGUSR2 == lastSignal) {
// start scan, no filter
scanning = 1;
hci_le_set_scan_enable(hciSocket, 0x00, 0, 1000);
hci_le_set_scan_enable(hciSocket, 0x01, 0, 1000);
} else if (SIGHUP == lastSignal) {
// stop scan
scanning = 0;
hci_le_set_scan_enable(hciSocket, 0x00, 0, 1000);
}
} else if (selectRetval) {
// read event
hciEventLen = read(hciSocket, hciEventBuf, sizeof(hciEventBuf));
leMetaEvent = (evt_le_meta_event *)(hciEventBuf + (1 + HCI_EVENT_HDR_SIZE));
hciEventLen -= (1 + HCI_EVENT_HDR_SIZE);
if (!scanning) {
// ignore, not scanning
continue;
}
if (leMetaEvent->subevent != 0x02) {
continue;
}
leAdvertisingInfo = (le_advertising_info *)(leMetaEvent->data + 1);
ba2str(&leAdvertisingInfo->bdaddr, btAddress);
printf("event %s,%s,", btAddress, (leAdvertisingInfo->bdaddr_type == LE_PUBLIC_ADDRESS) ? "public" : "random");
for (i = 0; i < leAdvertisingInfo->length; i++) {
printf("%02x", leAdvertisingInfo->data[i]);
}
rssi = *(leAdvertisingInfo->data + leAdvertisingInfo->length);
printf(",%d\n", rssi);
}
}
// restore original filter
setsockopt(hciSocket, SOL_HCI, HCI_FILTER, &oldHciFilter, sizeof(oldHciFilter));
// disable LE scan
hci_le_set_scan_enable(hciSocket, 0x00, 0, 1000);
close(hciSocket);
return 0;
}
int main(int argc, const char* argv[])
{
const char *hciDeviceIdOverride = NULL;
int hciDeviceId = 0;
int hciSocket;
struct hci_dev_info hciDevInfo;
int previousAdapterState = -1;
int currentAdapterState;
const char* adapterState = NULL;
fd_set rfds;
struct timeval tv;
int selectRetval;
char stdinBuf[256 * 2 + 1];
char advertisementDataBuf[256];
int advertisementDataLen = 0;
char scanDataBuf[256];
int scanDataLen = 0;
int len;
int i;
memset(&hciDevInfo, 0x00, sizeof(hciDevInfo));
// 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);
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);
// setup HCI socket
hciSocket = hci_open_dev(hciDeviceId);
hciDevInfo.dev_id = hciDeviceId;
if (hciSocket == -1) {
printf("adapterState unsupported\n");
return -1;
}
while(1) {
FD_ZERO(&rfds);
FD_SET(0, &rfds);
FD_SET(hciSocket, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
// get HCI dev info for adapter state
ioctl(hciSocket, HCIGETDEVINFO, (void *)&hciDevInfo);
currentAdapterState = hci_test_bit(HCI_UP, &hciDevInfo.flags);
if (previousAdapterState != currentAdapterState) {
previousAdapterState = currentAdapterState;
if (!currentAdapterState) {
adapterState = "poweredOff";
} else {
hci_le_set_advertise_enable(hciSocket, 0, 1000);
hci_le_set_advertise_enable(hciSocket, 1, 1000);
if (hci_le_set_advertise_enable(hciSocket, 0, 1000) == -1) {
switch errno {
case EPERM:
adapterState = "unauthorized";
break;
case EIO:
adapterState = "unsupported";
break;
case ETIMEDOUT:
adapterState = "timedout";
break;
default:
printf("advertiseErrno %d\n", errno);
adapterState = "unknown";
break;
}
} else {
adapterState = "poweredOn";
}
}
printf("adapterState %s\n", adapterState);
}
static DBusMessage *old_list_adapters(DBusConnection *conn,
DBusMessage *msg, void *data)
{
DBusMessageIter iter;
DBusMessageIter array_iter;
DBusMessage *reply;
struct hci_dev_list_req *dl;
struct hci_dev_req *dr;
int i, sk;
if (!dbus_message_has_signature(msg, DBUS_TYPE_INVALID_AS_STRING))
return invalid_args(msg);
sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
if (sk < 0)
return failed_strerror(msg, errno);
dl = g_malloc0(HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl));
dl->dev_num = HCI_MAX_DEV;
dr = dl->dev_req;
if (ioctl(sk, HCIGETDEVLIST, dl) < 0) {
int err = errno;
close(sk);
g_free(dl);
return failed_strerror(msg, err);
}
dr = dl->dev_req;
reply = dbus_message_new_method_return(msg);
if (!reply) {
close(sk);
g_free(dl);
return NULL;
}
dbus_message_iter_init_append(reply, &iter);
dbus_message_iter_open_container(&iter, DBUS_TYPE_ARRAY,
DBUS_TYPE_STRING_AS_STRING, &array_iter);
for (i = 0; i < dl->dev_num; i++, dr++) {
char path[MAX_PATH_LENGTH], *path_ptr = path;
struct hci_dev_info di;
if (hci_devinfo(dr->dev_id, &di) < 0)
continue;
if (hci_test_bit(HCI_RAW, &di.flags))
continue;
snprintf(path, sizeof(path), "%s/%s", BASE_PATH, di.name);
dbus_message_iter_append_basic(&array_iter,
DBUS_TYPE_STRING, &path_ptr);
}
dbus_message_iter_close_container(&iter, &array_iter);
g_free(dl);
close(sk);
return reply;
}
static void init_device(int index)
{
struct hci_dev_req dr;
struct hci_dev_info di;
pid_t pid;
int dd, err;
/* Do initialization in the separate process */
pid = fork();
switch (pid) {
case 0:
atexit(at_child_exit);
break;
case -1:
err = errno;
error("Fork failed. Can't init device hci%d: %s (%d)",
index, strerror(err), err);
default:
debug("child %d forked", pid);
return;
}
dd = hci_open_dev(index);
if (dd < 0) {
err = errno;
error("Can't open device hci%d: %s (%d)",
index, strerror(err), err);
exit(1);
}
memset(&dr, 0, sizeof(dr));
dr.dev_id = index;
/* Set link mode */
dr.dev_opt = main_opts.link_mode;
if (ioctl(dd, HCISETLINKMODE, (unsigned long) &dr) < 0) {
err = errno;
error("Can't set link mode on hci%d: %s (%d)",
index, strerror(err), err);
}
/* Set link policy */
dr.dev_opt = main_opts.link_policy;
if (ioctl(dd, HCISETLINKPOL, (unsigned long) &dr) < 0 &&
errno != ENETDOWN) {
error("Can't set link policy on hci%d: %s (%d)",
index, strerror(errno), errno);
}
/* Start HCI device */
if (ioctl(dd, HCIDEVUP, index) < 0 && errno != EALREADY) {
error("Can't init device hci%d: %s (%d)",
index, strerror(errno), errno);
goto fail;
}
if (hci_devinfo(index, &di) < 0)
goto fail;
if (hci_test_bit(HCI_RAW, &di.flags))
goto done;
done:
hci_close_dev(dd);
exit(0);
fail:
hci_close_dev(dd);
exit(1);
}
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;
}
static void configure_device(int dev_id)
{
struct device_opts *device_opts;
struct hci_dev_req dr;
struct hci_dev_info di;
char mode[14];
int dd;
device_opts = get_device_opts(dev_id);
if (hci_devinfo(dev_id, &di) < 0)
return;
if (hci_test_bit(HCI_RAW, &di.flags))
return;
/* Set default discoverable timeout if not set */
if (!(device_opts->flags & (1 << HCID_SET_DISCOVTO)))
device_opts->discovto = HCID_DEFAULT_DISCOVERABLE_TIMEOUT;
/* Set scan mode */
if (read_device_mode(&di.bdaddr, mode, sizeof(mode)) == 0) {
if (!strcmp(mode, "off") && hcid.offmode == HCID_OFFMODE_NOSCAN) {
device_opts->mode = MODE_OFF;
device_opts->scan = SCAN_DISABLED;
} else if (!strcmp(mode, "connectable")) {
device_opts->mode = MODE_CONNECTABLE;
device_opts->scan = SCAN_PAGE;
} else if (!strcmp(mode, "discoverable")) {
/* Set discoverable only if timeout is 0 */
if (!get_discoverable_timeout(dev_id)) {
device_opts->scan = SCAN_PAGE | SCAN_INQUIRY;
device_opts->mode = MODE_DISCOVERABLE;
} else {
device_opts->scan = SCAN_PAGE;
device_opts->mode = MODE_CONNECTABLE;
}
} else if (!strcmp(mode, "limited")) {
/* Set discoverable only if timeout is 0 */
if (!get_discoverable_timeout(dev_id)) {
device_opts->scan = SCAN_PAGE | SCAN_INQUIRY;
device_opts->mode = MODE_LIMITED;
} else {
device_opts->scan = SCAN_PAGE;
device_opts->mode = MODE_CONNECTABLE;
}
}
}
/* Do configuration in the separate process */
switch (fork()) {
case 0:
atexit(at_child_exit);
break;
case -1:
error("Fork failed. Can't init device hci%d: %s (%d)",
dev_id, strerror(errno), errno);
default:
return;
}
dd = hci_open_dev(dev_id);
if (dd < 0) {
error("Can't open device hci%d: %s (%d)",
dev_id, strerror(errno), errno);
exit(1);
}
memset(&dr, 0, sizeof(dr));
dr.dev_id = dev_id;
/* Set packet type */
if ((device_opts->flags & (1 << HCID_SET_PTYPE))) {
dr.dev_opt = device_opts->pkt_type;
if (ioctl(dd, HCISETPTYPE, (unsigned long) &dr) < 0) {
error("Can't set packet type on hci%d: %s (%d)",
dev_id, strerror(errno), errno);
}
}
/* Set link mode */
if ((device_opts->flags & (1 << HCID_SET_LM))) {
dr.dev_opt = device_opts->link_mode;
if (ioctl(dd, HCISETLINKMODE, (unsigned long) &dr) < 0) {
error("Can't set link mode on hci%d: %s (%d)",
dev_id, strerror(errno), errno);
}
}
/* Set link policy */
if ((device_opts->flags & (1 << HCID_SET_LP))) {
dr.dev_opt = device_opts->link_policy;
if (ioctl(dd, HCISETLINKPOL, (unsigned long) &dr) < 0) {
error("Can't set link policy on hci%d: %s (%d)",
dev_id, strerror(errno), errno);
}
}
/* Set device name */
if ((device_opts->flags & (1 << HCID_SET_NAME)) && device_opts->name) {
change_local_name_cp cp;
memset(cp.name, 0, sizeof(cp.name));
expand_name((char *) cp.name, sizeof(cp.name),
device_opts->name, dev_id);
hci_send_cmd(dd, OGF_HOST_CTL, OCF_CHANGE_LOCAL_NAME,
CHANGE_LOCAL_NAME_CP_SIZE, &cp);
}
/* Set device class */
if ((device_opts->flags & (1 << HCID_SET_CLASS))) {
write_class_of_dev_cp cp;
uint32_t class;
uint8_t cls[3];
if (read_local_class(&di.bdaddr, cls) < 0) {
class = htobl(device_opts->class);
cls[2] = get_service_classes(&di.bdaddr);
memcpy(cp.dev_class, &class, 3);
} else {
if (!(device_opts->scan & SCAN_INQUIRY))
static int hci_sock_sendmsg(struct socket *sock, struct msghdr *msg, int len,
struct scm_cookie *scm)
{
struct sock *sk = sock->sk;
struct hci_dev *hdev;
struct sk_buff *skb;
int err;
BT_DBG("sock %p sk %p", sock, sk);
if (msg->msg_flags & MSG_OOB)
return -EOPNOTSUPP;
if (msg->msg_flags & ~(MSG_DONTWAIT|MSG_NOSIGNAL|MSG_ERRQUEUE))
return -EINVAL;
if (len < 4)
return -EINVAL;
lock_sock(sk);
if (!(hdev = hci_pi(sk)->hdev)) {
err = -EBADFD;
goto done;
}
if (!(skb = bluez_skb_send_alloc(sk, len, msg->msg_flags & MSG_DONTWAIT, &err)))
goto done;
if (memcpy_fromiovec(skb_put(skb, len), msg->msg_iov, len)) {
err = -EFAULT;
goto drop;
}
skb->pkt_type = *((unsigned char *) skb->data);
skb_pull(skb, 1);
skb->dev = (void *) hdev;
if (skb->pkt_type == HCI_COMMAND_PKT) {
u16 opcode = __le16_to_cpu(get_unaligned((u16 *)skb->data));
u16 ogf = cmd_opcode_ogf(opcode);
u16 ocf = cmd_opcode_ocf(opcode);
if (((ogf > HCI_SFLT_MAX_OGF) ||
!hci_test_bit(ocf & HCI_FLT_OCF_BITS, &hci_sec_filter.ocf_mask[ogf])) &&
!capable(CAP_NET_RAW)) {
err = -EPERM;
goto drop;
}
if (test_bit(HCI_RAW, &hdev->flags) || (ogf == OGF_VENDOR_CMD)) {
skb_queue_tail(&hdev->raw_q, skb);
hci_sched_tx(hdev);
} else {
skb_queue_tail(&hdev->cmd_q, skb);
hci_sched_cmd(hdev);
}
} else {
if (!capable(CAP_NET_RAW)) {
err = -EPERM;
goto drop;
}
skb_queue_tail(&hdev->raw_q, skb);
hci_sched_tx(hdev);
}
err = len;
done:
release_sock(sk);
return err;
drop:
kfree_skb(skb);
goto done;
}
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;
}
static int run_proxy(int fd, int dev, bdaddr_t *bdaddr)
{
unsigned char buf[HCI_MAX_FRAME_SIZE + 1];
struct hci_dev_info di;
struct hci_filter flt;
struct pollfd p[2];
int dd, err, len, need_raw;
dd = hci_open_dev(dev);
if (dd < 0) {
syslog(LOG_ERR, "Can't open device hci%d: %s (%d)",
dev, strerror(errno), errno);
return 1;
}
if (hci_devinfo(dev, &di) < 0) {
syslog(LOG_ERR, "Can't get device info for hci%d: %s (%d)",
dev, strerror(errno), errno);
hci_close_dev(dd);
return 1;
}
need_raw = !hci_test_bit(HCI_RAW, &di.flags);
hci_filter_clear(&flt);
hci_filter_all_ptypes(&flt);
hci_filter_all_events(&flt);
if (setsockopt(dd, SOL_HCI, HCI_FILTER, &flt, sizeof(flt)) < 0) {
syslog(LOG_ERR, "Can't set filter for hci%d: %s (%d)",
dev, strerror(errno), errno);
hci_close_dev(dd);
return 1;
}
if (need_raw) {
if (ioctl(dd, HCISETRAW, 1) < 0) {
syslog(LOG_ERR, "Can't set raw mode on hci%d: %s (%d)",
dev, strerror(errno), errno);
hci_close_dev(dd);
return 1;
}
}
p[0].fd = fd;
p[0].events = POLLIN;
p[1].fd = dd;
p[1].events = POLLIN;
while (!__io_canceled) {
p[0].revents = 0;
p[1].revents = 0;
err = poll(p, 2, 500);
if (err < 0)
break;
if (!err)
continue;
if (p[0].revents & POLLIN) {
len = read(fd, buf, sizeof(buf));
if (len > 0) {
rewrite_bdaddr(buf, len, bdaddr);
err = write(dd, buf, len);
}
}
if (p[1].revents & POLLIN) {
len = read(dd, buf, sizeof(buf));
if (len > 0) {
rewrite_bdaddr(buf, len, bdaddr);
err = write(fd, buf, len);
}
}
}
if (need_raw) {
if (ioctl(dd, HCISETRAW, 0) < 0)
syslog(LOG_ERR, "Can't clear raw mode on hci%d: %s (%d)",
dev, strerror(errno), errno);
}
hci_close_dev(dd);
syslog(LOG_INFO, "Exit");
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;
}