void peisk_periodic_bluetoothSayHello(void *data) {
int i,j,len,len2;
static int status=0;
static int helloDevice=-1;
static int helloFd;
struct sockaddr_l2 addr = { 0 };
char buf[1024];
static double helloTimeout=0;
/** 0, nothinging happening. 1, started connecting, 2 waiting for
reply */
static int mode = 0;
if(mode == 0) {
/* Find the first bluetooth device to say hello to, if any */
for(i=0;i<peisk_nBluetoothDevices;i++)
if(peisk_timeNow > peisk_bluetoothDevices[i].nextHelloAttempt &&
peisk_bluetoothDevices[i].isConnectable == 0) {
for(j=0;j<peisk_nBluetoothAdaptors;j++)
if(peisk_timeNow > peisk_bluetoothDevices[i].lastSeen[j] - PEISK_BLUETOOTH_KEEPALIVE) break;
if(j != peisk_nBluetoothAdaptors) break;
}
if(i == peisk_nBluetoothDevices)
/* No device found, nothing to do */
return;
/* Remember which device we attempting to say hi to */
helloDevice=i;
/* Update when he should next be greeted */
peisk_bluetoothDevices[i].nextHelloAttempt = peisk_timeNow + PEISK_HELLO_TRYAGAIN;
/*printf("Saying HELLO to %02X:%02X:%02X:%02X:%02X:%02X\n",
peisk_bluetoothDevices[i].btAddr[0],peisk_bluetoothDevices[i].btAddr[1],peisk_bluetoothDevices[i].btAddr[2],
peisk_bluetoothDevices[i].btAddr[3],peisk_bluetoothDevices[i].btAddr[4],peisk_bluetoothDevices[i].btAddr[5]);
*/
helloFd = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
/* Bind the bluetooth interface with highest signal strength to
socket */
addr.l2_family = AF_BLUETOOTH;
addr.l2_psm = htobs(-1);
int bestStrength=0, bestAdaptor=-1;
for(j=0;j<peisk_nBluetoothAdaptors;j++)
if(peisk_timeNow > peisk_bluetoothDevices[i].lastSeen[j] - PEISK_BLUETOOTH_KEEPALIVE &&
peisk_bluetoothDevices[i].rawStrength[j] > bestStrength) {
bestStrength=peisk_bluetoothDevices[i].rawStrength[j];
bestAdaptor=j;
}
PEISK_ASSERT(bestAdaptor!=-1,("No adaptor found? should not be possible\n"));
addr.l2_bdaddr = peisk_bluetoothAdaptors[bestAdaptor].addr;
bind(helloFd, (struct sockaddr *)&addr, sizeof(addr));
/*ba2str(&addr.l2_bdaddr,buf);
printf("Bound %s (%s) for outgoing HELLO connection\n",peisk_bluetoothAdaptors[bestAdaptor].name,buf);*/
/* Establish a connection */
addr.l2_family = AF_BLUETOOTH;
addr.l2_psm = htobs(0x1001);
for(j=0;j<6;j++)
addr.l2_bdaddr.b[j] = peisk_bluetoothDevices[i].btAddr[5-j];
ba2str( &addr.l2_bdaddr, buf );
printf("Starting HELLO connection to %s\n",buf);
if(fcntl(helloFd,F_SETFL,O_NONBLOCK) != 0) {
perror("Failed to set bluetooth socket nonblocking\n");
}
connect(helloFd, (struct sockaddr *)&addr, sizeof(addr));
helloTimeout = peisk_timeNow + PEISK_HELLO_TIMEOUT;
mode = 1;
}
if(mode == 1) {
i = helloDevice;
struct pollfd fds;
fds.fd = helloFd;
fds.events = -1;
fds.revents = 0;
if(poll(&fds,1,0)) {
socklen_t slen;
slen=sizeof(status);
if(getsockopt(helloFd,SOL_SOCKET,SO_ERROR,(void*) &status,&slen)) {
printf("Get sockopt failed...\n");
}
/*printf("Connection finished: status=%d (time elapsed: %.3fs)\n",status,peisk_gettimef()-helloTimeout+10.0);*/
if(status) {
/*printf("Failed to say HELLO\n");*/
close(helloFd);
mode = 0;
helloFd = -1;
helloDevice = -1;
} else {
len=peisk_bluetoothMakeHELLO(buf);
/*printf("Sending %d bytes HELLO\n",len);*/
len2=write(helloFd,buf,len);
PEISK_ASSERT(len2 == len,("Wrote only %d of %d bytes in HELLO message\n",len2,len));
mode = 2;
}
}
}
if(mode == 2) {
i = helloDevice;
len = read(helloFd,buf,sizeof(buf));
if(len > 0) {
/*printf("Got HELLO response\n",buf);*/
peisk_bluetoothParseHELLO(buf,len);
peisk_bluetoothDevices[i].isConnectable = 1;
/*printf("Outgoing ");
for(j=0;j<6;j++) printf("%02X:",peisk_bluetoothDevices[i].btAddr[j]);
printf(" is a PEIS\n");*/
close(helloFd);
mode = 0;
helloFd = -1;
helloDevice = -1;
}
}
if(mode > 0 && helloTimeout < peisk_timeNow) {
/*printf("Giving up... %.1fs passed\n",peisk_timeNow-(helloTimeout-PEISK_HELLO_TIMEOUT));*/
close(helloFd);
mode = 0;
helloFd = -1;
helloDevice = -1;
}
}
int main(int argc, char **argv)
{
fprintf(stderr, "Launch the app BTMouse in your phone and connect with the PC! \n");
struct sockaddr_rc loc_addr = { 0 }, rem_addr = { 0 };
char buf[1024] = { 0 };
int s, client, bytes_read,flag=1;
socklen_t opt = sizeof(rem_addr);
// allocate socket
s = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
// bind socket to port 1 of the first available
// local bluetooth adapter
loc_addr.rc_family = AF_BLUETOOTH;
loc_addr.rc_bdaddr = *BDADDR_ANY;
loc_addr.rc_channel = (uint8_t) 1;
bind(s, (struct sockaddr *)&loc_addr, sizeof(loc_addr));
// put socket into listening mode
listen(s, 1);
// accept one connection
client = accept(s, (struct sockaddr *)&rem_addr, &opt);
ba2str( &rem_addr.rc_bdaddr, buf );
fprintf(stderr, "accepted connection from %s\n", buf);
memset(buf, 0, sizeof(buf));
// initialise X components
dpy = XOpenDisplay(0);
root_window = XRootWindow(dpy, 0);
XSelectInput(dpy, root_window, KeyReleaseMask);
// read data from the client
while(1){
bytes_read = read(client, buf, sizeof(buf));
if( bytes_read > 0 ) {
//if(flag==1){
printf("%s \n", buf);
//flag=2;}
switch(buf[0]){
case 'l':
mouseClick(1);
break;
case 'r':
mouseClick(3);
break;
case '(':
act(buf);
break;
}}
else {
}
}
// close connection
close(client);
close(s);
//move(100,100);
//refresh();
return 0;
}
// Informacion sobre el dispositivo
string TPFC_device_wiimote::btaddress(){
char aux[100];
ba2str(&bdaddr, aux);
return aux;
}
static void getDevices(std::vector<al::Bluetooth>& devs){
/*
See cmd_inq and cmd_scan in tools/hcitool.c.
*/
/* From bluetooth/hci.h:
typedef struct {
bdaddr_t bdaddr;
uint8_t pscan_rep_mode;
uint8_t pscan_period_mode;
uint8_t pscan_mode;
uint8_t dev_class[3];
uint16_t clock_offset;
} __attribute__ ((packed)) inquiry_info;
*/
/*static const char *inq_help =
"Usage:\n"
"\tinq [--length=N] maximum inquiry duration in 1.28 s units\n"
"\t [--numrsp=N] specify maximum number of inquiry responses\n"
"\t [--iac=lap] specify the inquiry access code\n"
"\t [--flush] flush the inquiry cache\n";*/
// First determine if there is a Bluetooth controller
int dev_id = hci_get_route(NULL);
if (dev_id < 0) {
perror("Bluetooth not available");
return;
}
inquiry_info *info = NULL;
uint8_t lap[3] = { 0x33, 0x8b, 0x9e };
int num_rsp = 16; // max number responses
int length = 8; // max inquiry response time, in 1.28 s
int flags = 0;
char addr[18], name[249];
num_rsp = hci_inquiry(dev_id, length, num_rsp, lap, &info, flags);
if (num_rsp < 0) {
perror("Bluetooth HCI inquiry failed.");
return;
}
int dd = hci_open_dev(dev_id);
if (dd < 0) {
perror("Bluetooth HCI device open failed");
bt_free(info);
return;
}
Bluetooth bt;
for (int i = 0; i < num_rsp; i++) {
ba2str(&info[i].bdaddr, addr);
if(hci_read_remote_name_with_clock_offset(dd,
&info[i].bdaddr,
info[i].pscan_rep_mode,
info[i].clock_offset | 0x8000,
sizeof(name), name, 100000)
< 0){
strcpy(name, "n/a");
}
bt.mName = std::string(name);
bt.mAddr = std::string(addr);
bt.mClass= (unsigned(info[i].dev_class[2])<<16) | (unsigned(info[i].dev_class[1])<<8) | info[i].dev_class[0];
devs.push_back(bt);
}
/**/
bt_free(info);
}
static void report_map_read_cb(guint8 status, const guint8 *pdu, guint16 plen,
gpointer user_data)
{
struct hog_device *hogdev = user_data;
struct btd_adapter *adapter = device_get_adapter(hogdev->device);
uint8_t value[HOG_REPORT_MAP_MAX_SIZE];
struct uhid_event ev;
uint16_t vendor_src, vendor, product, version;
ssize_t vlen;
char itemstr[20]; /* 5x3 (data) + 4 (continuation) + 1 (null) */
int i, err;
GSList *l;
if (status != 0) {
error("Report Map read failed: %s", att_ecode2str(status));
return;
}
vlen = dec_read_resp(pdu, plen, value, sizeof(value));
if (vlen < 0) {
error("ATT protocol error");
return;
}
DBG("Report MAP:");
for (i = 0; i < vlen;) {
ssize_t ilen = 0;
bool long_item = false;
if (get_descriptor_item_info(&value[i], vlen - i, &ilen,
&long_item)) {
/* Report ID is short item with prefix 100001xx */
if (!long_item && (value[i] & 0xfc) == 0x84)
hogdev->has_report_id = TRUE;
DBG("\t%s", item2string(itemstr, &value[i], ilen));
i += ilen;
} else {
error("Report Map parsing failed at %d", i);
/* Just print remaining items at once and break */
DBG("\t%s", item2string(itemstr, &value[i], vlen - i));
break;
}
}
vendor_src = btd_device_get_vendor_src(hogdev->device);
vendor = btd_device_get_vendor(hogdev->device);
product = btd_device_get_product(hogdev->device);
version = btd_device_get_version(hogdev->device);
DBG("DIS information: vendor_src=0x%X, vendor=0x%X, product=0x%X, "
"version=0x%X", vendor_src, vendor, product, version);
/* create uHID device */
memset(&ev, 0, sizeof(ev));
ev.type = UHID_CREATE;
if (device_name_known(hogdev->device))
device_get_name(hogdev->device, (char *) ev.u.create.name,
sizeof(ev.u.create.name));
else
strcpy((char *) ev.u.create.name, "bluez-hog-device");
ba2str(btd_adapter_get_address(adapter), (char *) ev.u.create.phys);
ba2str(device_get_address(hogdev->device), (char *) ev.u.create.uniq);
ev.u.create.vendor = vendor;
ev.u.create.product = product;
ev.u.create.version = version;
ev.u.create.country = hogdev->bcountrycode;
ev.u.create.bus = BUS_BLUETOOTH;
ev.u.create.rd_data = value;
ev.u.create.rd_size = vlen;
err = bt_uhid_send(hogdev->uhid, &ev);
if (err < 0) {
error("bt_uhid_send: %s", strerror(-err));
return;
}
bt_uhid_register(hogdev->uhid, UHID_OUTPUT, forward_report, hogdev);
bt_uhid_register(hogdev->uhid, UHID_SET_REPORT, set_report, hogdev);
bt_uhid_register(hogdev->uhid, UHID_GET_REPORT, get_report, hogdev);
hogdev->uhid_created = TRUE;
for (l = hogdev->reports; l; l = l->next) {
struct report *r = l->data;
enable_report_notifications(r, true);
}
}
bool scanBluetoothAndCommunicate(int robotId)
{
// open device
int devId = hci_get_route(NULL);
if (devId < 0)
{
std::cerr << "Error, can't get bluetooth adapter ID" << std::endl;
return false;
}
// open socket
int sock = hci_open_dev(devId);
if (sock < 0)
{
std::cerr << "Error,can't open bluetooth adapter" << std::endl;
return false;
}
// query
std::cout << "Scanning bluetooth:" << std::endl;
//int length = 8; /* ~10 seconds */
int length = 4; /* ~5 seconds */
inquiry_info *info = NULL;
// device id, query length (last 1.28 * length seconds), max devices, lap ??, returned array, flag
int devicesCount = hci_inquiry(devId, length, 255, NULL, &info, 0);
if (devicesCount < 0)
{
std::cerr << "Error, can't query bluetooth" << std::endl;
close(sock);
return false;
}
// print devices
int fd = -2;
for (int i = 0; i < devicesCount; i++)
{
char addrString[19];
char addrFriendlyName[256];
ba2str(&(info+i)->bdaddr, addrString);
if (hci_read_remote_name(sock, &(info+i)->bdaddr, 256, addrFriendlyName, 0) < 0)
strcpy(addrFriendlyName, "[unknown]");
printf("\t%s %s\n", addrString, addrFriendlyName);
if (strncmp("e-puck_", addrFriendlyName, 7) == 0)
{
int id;
sscanf(addrFriendlyName + 7, "%d", &id);
if (sscanf(addrFriendlyName + 7, "%d", &id) && (id == robotId))
{
std::cout << "Contacting e-puck " << id << std::endl;
fd = connectToEPuck(&(info+i)->bdaddr);
if (fd==-1)
{
std::cerr << "Error: e-puck " << robotId << " cannot connect" << std::endl;
break;
}
// TODO: read and write here
closeConnectionToEPuck(fd);
break;
}
}
}
if (fd==-2)
std::cerr << "Error: e-puck " << robotId << " not found" << std::endl;
free(info);
close(sock);
return fd;
}
int main (void)
{
DIR *dp;
btopush_dev_t devs[BTOPUSH_MAX_DEV];btopush_ctx_t btctx;
struct dirent *ep;
char filename[MAX_FILES][25],recv_addr[18],self_addr[18],sent_addr[18];
char tree_addr[18],recv_tree_addr[18],temp_addr[18],fname[30];
char filetype[5];
FILE *fp,*recv,*node,*list,*send,*sent_count;
int i=0,j,a,dev_id,self_node_status,recv_node_status,N,ch,devc;
int NDesc,recv_N,recv_NDesc,sent=0;
bdaddr_t ba,ba1;
dp = opendir ("./");
if (dp != NULL)
{
while (ep = readdir (dp))
if( strcmp(ep->d_name,".") != 0 && strcmp(ep->d_name,"..") != 0
&& strcmp(ep->d_name,"recvd") != 0)
{
strcpy(filename[i],ep->d_name);
i++;
}
(void) closedir (dp);
}
else
perror ("Couldn't open the directory");
j=i-1;
for( i=0; i<=j; i++)
{
/* Read the Node Address from the received filename[i] */
fp = fopen(filename[i],"r");
fgets(recv_addr,18,fp);
fseek(fp,1,1);
fgets(recv_tree_addr,18,fp);
fscanf(fp,"%d %d",&recv_node_status,&recv_N);
fclose(fp);
strncpy(filetype,filename[i],4);
printf("\n%s\n",filetype);
/* Reading the contents from node_status.conf */
node = fopen(NODE_STATUS_FILE1,"r");
fgets(self_addr,18,fp);
fseek(fp,1,1);
fgets(tree_addr,18,node);
fscanf(node,"%d %d %d",&self_node_status,&N,&NDesc);
fclose(node);
/* Tearing down the link if both nodes belong to same tree */
if( strcmp(tree_addr,recv_tree_addr) == 0 &&
(strcmp(tree_addr,"00:00:00:00:00:00"))!=0 ) break;
/* Update Message as Response for Init, Checking for case A2 */
else if( recv_node_status == FREE_NODE && strcmp(filetype,"Init")== 0)
{ printf("\ninside init\n");
if(self_node_status == FREE_NODE )
{
srand((unsigned int)time( NULL ));
if(rand()%2)
{
self_node_status = ROOT_NODE;
strcpy(tree_addr,self_addr);
N++;
node = fopen(NODE_STATUS_FILE1,"w");
fprintf(node,"%s %s %d %d %d\n",self_addr,tree_addr,self_node_status,N,NDesc);
fclose(node);
node = fopen(TOPOLOGY_FILE,"w");
fprintf(node,"%s\n",recv_addr);
fclose(node);
}
else
{
self_node_status = NON_ROOT_NODE;
strcpy(tree_addr,recv_addr);
N++;
node = fopen(NODE_STATUS_FILE1,"w");
fprintf(node,"%s %s %d %d %d\n",self_addr,tree_addr,self_node_status,N,NDesc);
fclose(node);
}
/*
str2ba(recv_addr,&ba);
str2ba(self_addr,&ba1);
ch = btopush_get_channel(&ba1, &ba);
if (ch != BTOPUSH_ERROR) {
memcpy(&devs.addr, (bdaddr_t *) recv_addr, sizeof(bdaddr_t));
devs.channel = ch;
printf("\n%d\n",ch);
}
*/
if ((devc = btopush_inq_objpush(devs)) <= 0)
{
fprintf(stderr, "could not find objpush capable devices\n");
return;
}
for(a=0;a<BTOPUSH_MAX_DEV;a++) /* Searching for recv_addr */
{
ba2str(&(devs[a].addr),temp_addr);
if( strcmp(recv_addr,temp_addr) == 0 )break;
}
if(a==BTOPUSH_MAX_DEV) /* If the recv_addr not found then its init ignored */
{
//unlink(filename[i]);
continue;
}
update:
strcpy(fname,UpdateParameters);
strcat(fname,self_addr);
send = fopen(fname,"w");
fprintf(send,"%s %s %d %d %d\n",self_addr,tree_addr,self_node_status,N,NDesc);
fclose(send);
sent++;
btopush_init(&btctx);
if (btopush_attach_dev(&btctx,(devs+a)) != BTOPUSH_SUCCESS)
{
fprintf(stderr, "%s could not set device\n", recv_addr);
goto disc;
}
if (btopush_open(&btctx) != BTOPUSH_SUCCESS)
{
fprintf(stderr, "%s could not open connection\n", recv_addr);
goto disc;
}
if (btopush_connect(&btctx, "prijsobject") != BTOPUSH_SUCCESS)
{
fprintf(stderr, "%s could not connect\n", recv_addr);
goto disc;
}
send = fopen(fname,"r");
fprintf(stdout, "%s start sending %s\n", recv_addr, fname);
if (btopush_open_file(&btctx, fname) != BTOPUSH_SUCCESS)
{
fprintf(stderr, "could not open file: %s\n", fname);
goto disc;
}
if (btopush_push_stream(&btctx) != BTOPUSH_SUCCESS)
{
if (btctx.req_state == BTOPUSH_REQS_TIMEOUT)
{
fprintf(stderr, "%s connection timed out\n", recv_addr);
btopush_close_file(&btctx);
goto disc;
}
else
{
fprintf(stderr, "%s cancelled\n", recv_addr);
btopush_close_file(&btctx);
goto disc;
}
}
else
{
fprintf(stdout, "%s stream succesfull\n", recv_addr);
}
btopush_disconnect(&btctx);
disc:
//unlink(filename[i]);
fclose(send);
continue;
} /* End of case A2 */
}/*
else if( recv_node_status == ROOT_NODE )
{
} */
/*Response to Update Message*/
else if(strcmp(filetype,"UPrm")== 0)
{
self_node_status = NON_ROOT_NODE;
if( strcmp(recv_tree_addr,self_addr) == 0 )
{
self_node_status = ROOT_NODE;
if(NDesc == -1 ) NDesc =1;
else NDesc++;
node = fopen(TOPOLOGY_FILE,"w");
fprintf(node,"%s\n",recv_addr);
fclose(node);
}
node = fopen("../node_status.conf","w");
fprintf(node,"%s %s %d %d %d\n",self_addr,recv_tree_addr,self_node_status,N,NDesc);
fclose(node);
break;
}
} /* for loop Ends */
sent_count = fopen(SENT_COUNT_FILE,"w");
fprintf(sent_count,"%d",sent);
fclose(sent_count);
return 0;
}
void WiimoteScannerLinux::FindWiimotes(std::vector<Wiimote*>& found_wiimotes, Wiimote*& found_board)
{
// supposedly 1.28 seconds
int const wait_len = 1;
int const max_infos = 255;
inquiry_info scan_infos[max_infos] = {};
auto* scan_infos_ptr = scan_infos;
found_board = nullptr;
// Use Limited Dedicated Inquiry Access Code (LIAC) to query, since third-party Wiimotes
// cannot be discovered without it.
const u8 lap[3] = {0x00, 0x8b, 0x9e};
// Scan for Bluetooth devices
int const found_devices =
hci_inquiry(m_device_id, wait_len, max_infos, lap, &scan_infos_ptr, IREQ_CACHE_FLUSH);
if (found_devices < 0)
{
ERROR_LOG(WIIMOTE, "Error searching for Bluetooth devices.");
return;
}
DEBUG_LOG(WIIMOTE, "Found %i Bluetooth device(s).", found_devices);
// Display discovered devices
for (int i = 0; i < found_devices; ++i)
{
NOTICE_LOG(WIIMOTE, "found a device...");
// BT names are a maximum of 248 bytes apparently
char name[255] = {};
if (hci_read_remote_name(m_device_sock, &scan_infos[i].bdaddr, sizeof(name), name, 1000) < 0)
{
ERROR_LOG(WIIMOTE, "name request failed");
continue;
}
NOTICE_LOG(WIIMOTE, "device name %s", name);
if (!IsValidDeviceName(name))
continue;
char bdaddr_str[18] = {};
ba2str(&scan_infos[i].bdaddr, bdaddr_str);
if (!IsNewWiimote(bdaddr_str))
continue;
// Found a new device
Wiimote* wm = new WiimoteLinux(scan_infos[i].bdaddr);
if (IsBalanceBoardName(name))
{
found_board = wm;
NOTICE_LOG(WIIMOTE, "Found balance board (%s).", bdaddr_str);
}
else
{
found_wiimotes.push_back(wm);
NOTICE_LOG(WIIMOTE, "Found Wiimote (%s).", bdaddr_str);
}
}
}
static void connect_cb(GIOChannel *chan, GError *err, gpointer user_data)
{
struct hid_device *dev;
bdaddr_t src, dst;
char address[18];
uint16_t psm;
GError *gerr = NULL;
GSList *l;
uuid_t uuid;
if (err) {
error("%s", err->message);
return;
}
bt_io_get(chan, &gerr,
BT_IO_OPT_SOURCE_BDADDR, &src,
BT_IO_OPT_DEST_BDADDR, &dst,
BT_IO_OPT_PSM, &psm,
BT_IO_OPT_INVALID);
if (gerr) {
error("%s", gerr->message);
g_io_channel_shutdown(chan, TRUE, NULL);
g_error_free(gerr);
return;
}
ba2str(&dst, address);
DBG("Incoming connection from %s on PSM %d", address, psm);
switch (psm) {
case L2CAP_PSM_HIDP_CTRL:
l = g_slist_find_custom(devices, &dst, device_cmp);
if (l)
return;
dev = g_new0(struct hid_device, 1);
bacpy(&dev->dst, &dst);
dev->ctrl_io = g_io_channel_ref(chan);
dev->uhid_fd = -1;
bt_string2uuid(&uuid, HID_UUID);
if (bt_search_service(&src, &dev->dst, &uuid,
hid_sdp_search_cb, dev, NULL, 0) < 0) {
error("failed to search sdp details");
hid_device_free(dev);
return;
}
devices = g_slist_append(devices, dev);
dev->ctrl_watch = g_io_add_watch(dev->ctrl_io,
G_IO_HUP | G_IO_ERR | G_IO_NVAL,
ctrl_watch_cb, dev);
bt_hid_notify_state(dev, HAL_HIDHOST_STATE_CONNECTING);
break;
case L2CAP_PSM_HIDP_INTR:
l = g_slist_find_custom(devices, &dst, device_cmp);
if (!l)
return;
dev = l->data;
dev->intr_io = g_io_channel_ref(chan);
dev->intr_watch = g_io_add_watch(dev->intr_io,
G_IO_IN | G_IO_HUP | G_IO_ERR | G_IO_NVAL,
intr_watch_cb, dev);
bt_hid_notify_state(dev, HAL_HIDHOST_STATE_CONNECTED);
break;
}
}
/* fix to support with rssi */
void reporter_add_with_rssi(inquiry_info_with_rssi* newthingy) {
char ieeeaddr[18];
int i;
inquiry_info_with_rssi *rstore;
//debug && printf("\n\n --- BEFORE BACMP \n");
for (i = 0; i < entries1; i++) {
//debug && printf("\n\n ------ COMPARE ITERATION: %d \n",i);
if (0 == bacmp(&((storeR1+i)->bdaddr),&(newthingy->bdaddr))) {
(storeR1+i)->clock_offset = (storeR1+i)->clock_offset + abs(newthingy->rssi);
(storeR1+i)->pscan_rep_mode = (storeR1+i)->pscan_rep_mode + 1;
if (debug) {
ba2str(&(newthingy->bdaddr), ieeeaddr);
printf("= %s %02x %04x ", ieeeaddr,
newthingy->pscan_rep_mode,
newthingy->clock_offset);
classinfo(newthingy->dev_class);
printf("\n");
}
return;
}
}
//debug && printf("\n\n --- BEFORE REALLOCING STORER1 \n");
entries1++;
if ((entries1 * INQUIRY_INFO_WITH_RSSI_SIZE) > capacity1) {
capacity1 = entries1 * INQUIRY_INFO_WITH_RSSI_SIZE;
rstore = realloc(storeR1, capacity1);
if (rstore == NULL) {
perror("I cannot allocate any more memory for new device, but continuing anyways... :-/");
return;
}
storeR1 = rstore;
}
//debug && printf("\n\n --- BEFORE STORING IN ARRAY \n");
/* store in the array */
newthingy->pscan_rep_mode = 1;
newthingy->clock_offset = abs(newthingy->rssi);
*(storeR1+(entries1-1)) = *newthingy;
//debug && printf("\n\n --- BEFORE BACMP2 \n");
/* find out if new object is in the old array... C cannot compare structures on its own... */
for (i = 0; i < entries2; i++) {
if (0 == bacmp(&((store2+i)->bdaddr),&(newthingy->bdaddr))) {
if (debug) {
ba2str(&(newthingy->bdaddr), ieeeaddr);
printf("= %s %02x %04x ", ieeeaddr,
newthingy->pscan_rep_mode,
newthingy->clock_offset);
classinfo(newthingy->dev_class);
printf("\n");
}
return;
}
}
//reporter_add(&template);
}
int main(int argc, char *argv[])
{
// Handle signals
signal(SIGINT,shut_down);
signal(SIGHUP,shut_down);
signal(SIGTERM,shut_down);
signal(SIGQUIT,shut_down);
// HCI device number, MAC struct
int device = 0;
bdaddr_t bdaddr;
bacpy(&bdaddr, BDADDR_ANY);
// Time to scan. Scan time is roughly 1.28 seconds * scan_window
// Originally this was always 8, now we adjust based on device:
#ifdef OPENWRT
int scan_window = 8;
#elif PWNPLUG
int scan_window = 5;
#else
int scan_window = 3;
#endif
// Maximum number of devices per scan
int max_results = 255;
int num_results;
// Device cache and index
int cache_index = 0;
// HCI cache setting
int flags = IREQ_CACHE_FLUSH;
// Strings to hold MAC and name
char addr[19] = {0};
char addr_buff[19] = {0};
// String for time
char cur_time[20];
// Process ID read from PID file
int ext_pid;
// Pointers to filenames
char *infofilename = LIVE_INF;
// Change default filename based on date
char OUT_FILE[1000] = OUT_PATH;
strncat(OUT_FILE, file_timestamp(),sizeof(OUT_FILE)-strlen(OUT_FILE)-1);
char *outfilename = OUT_FILE;
// Mode to open output file in
char *filemode = "a+";
// Output buffer
char outbuffer[500];
// Buffer for data from the second loop
char exitbuffer[500];
// Misc Variables
int i, ri, opt;
// Record numbner of BlueZ errors
int error_count = 0;
// Current epoch time
long long int epoch;
// Kernel version info
struct utsname sysinfo;
uname(&sysinfo);
while ((opt=getopt_long(argc,argv,"+o:i:r:a:w:vxctghldbfenksmq", main_options, NULL)) != EOF)
{
switch (opt)
{
case 'i':
if (!strncasecmp(optarg, "hci", 3))
hci_devba(atoi(optarg + 3), &bdaddr);
else
str2ba(optarg, &bdaddr);
break;
case 'o':
outfilename = strdup(optarg);
break;
case 'r':
config.retry_count = atoi(optarg);
break;
case 'a':
config.amnesia = atoi(optarg);
break;
case 'w':
config.scan_window = round((atoi(optarg) / 1.28));
break;
case 'c':
config.showclass = 1;
break;
case 'e':
config.encode = 1;
break;
case 'f':
config.friendlyclass = 1;
break;
case 'v':
config.verbose = 1;
break;
case 'g':
config.status = 1;
break;
case 't':
config.showtime = 1;
break;
case 's':
config.syslogonly = 1;
break;
case 'x':
config.obfuscate = 1;
break;
case 'q':
config.quiet = 1;
break;
case 'l':
if(!LIVEMODE)
{
printf("Live mode has been disabled in this build. See documentation.\n");
exit(0);
}
else
config.bluelive = 1;
break;
case 'b':
config.bluepropro = 1;
break;
case 'd':
config.daemon = 1;
break;
case 'n':
config.getname = 1;
break;
case 'm':
if(!OUILOOKUP)
{
printf("Manufacturer lookups have been disabled in this build. See documentation.\n");
exit(0);
}
else
config.getmanufacturer = 1;
break;
case 'h':
help();
exit(0);
case 'k':
// Read PID from file into variable
ext_pid = read_pid();
if (ext_pid != 0)
{
printf("Killing Bluelog process with PID %i...",ext_pid);
if(kill(ext_pid,15) != 0)
{
printf("ERROR!\n");
printf("Unable to kill Bluelog process. Check permissions.\n");
exit(1);
}
else
printf("OK.\n");
// Delete PID file
unlink(PID_FILE);
}
else
printf("No running Bluelog process found.\n");
exit(0);
default:
printf("Unknown option. Use -h for help, or see README.\n");
exit(1);
}
}
// See if there is already a process running
if (read_pid() != 0)
{
printf("Another instance of Bluelog is already running!\n");
printf("Use the -k option to kill a running Bluelog process.\n");
exit(1);
}
// Load config from file if no options given on command line
if(cfg_exists() && argc == 1)
{
if (cfg_read() != 0)
{
printf("Error opening config file!\n");
exit(1);
}
// Put interface into BT struct
hci_devba(config.hci_device, &bdaddr);
}
// Perform sanity checks on varibles
cfg_check();
// Setup libmackerel
mac_init();
// Boilerplate
if (!config.quiet)
{
printf("%s (v%s%s) by MS3FGX\n", APPNAME, VERSION, VER_MOD);
#if defined OPENWRT || PWNPLUG
printf("----");
#endif
printf("---------------------------\n");
}
// Show notification we loaded config from file
if(cfg_exists() && argc == 1 && !config.quiet)
printf("Config loaded from: %s\n", CFG_FILE);
// Init Hardware
ba2str(&bdaddr, config.addr);
if (!strcmp(config.addr, "00:00:00:00:00:00"))
{
if (!config.quiet)
printf("Autodetecting device...");
device = hci_get_route(NULL);
// Put autodetected device MAC into addr
hci_devba(device, &bdaddr);
ba2str(&bdaddr, config.addr);
}
else
{
if (!config.quiet)
printf("Initializing device...");
device = hci_devid(config.addr);
}
// Open device and catch errors
config.bt_socket = hci_open_dev(device);
if (device < 0 || config.bt_socket < 0)
{
// Failed to open device, that can't be good
printf("\n");
printf("Error initializing Bluetooth device!\n");
exit(1);
}
// If we get here the device should be online.
if (!config.quiet)
printf("OK\n");
// Status message for BPP
if (!config.quiet)
if (config.bluepropro)
printf("Output formatted for BlueProPro.\n"
"More Info: www.hackfromacave.com\n");
// Open socket
if (config.udponly)
open_udp_socket();
// Open output file, unless in networking mode
if (!config.syslogonly && !config.udponly)
{
if (config.bluelive)
{
// Change location of output file
outfilename = LIVE_OUT;
filemode = "w";
if (!config.quiet)
printf("Starting Bluelog Live...\n");
}
if (!config.quiet)
printf("Opening output file: %s...", outfilename);
if ((outfile = fopen(outfilename, filemode)) == NULL)
{
printf("\n");
printf("Error opening output file!\n");
exit(1);
}
if (!config.quiet)
printf("OK\n");
}
else
if (!config.quiet)
printf("Network mode enabled, not creating log file.\n");
// Open status file
if (config.bluelive)
{
if (!config.quiet)
printf("Opening info file: %s...", infofilename);
if ((infofile = fopen(infofilename,"w")) == NULL)
{
printf("\n");
printf("Error opening info file!\n");
exit(1);
}
if (!config.quiet)
printf("OK\n");
}
// Write PID file
if (!config.daemon)
write_pid(getpid());
// Get and print time to console and file
strcpy(cur_time, get_localtime());
if (!config.daemon)
printf("Scan started at [%s] on %s\n", cur_time, config.addr);
if (config.showtime && (outfile != NULL))
{
fprintf(outfile,"[%s] Scan started on %s\n", cur_time, config.addr);
// Make sure this gets written out
fflush(outfile);
}
// Write info file for Bluelog Live
if (config.bluelive)
{
fprintf(infofile,"<div class=\"sideitem\">%s Version: %s%s</div>\n", APPNAME, VERSION, VER_MOD);
fprintf(infofile,"<div class=\"sideitem\">Device: %s</div>\n", config.addr);
fprintf(infofile,"<div class=\"sideitem\">Started: %s</div>\n", cur_time);
// Think we are done with you now
fclose(infofile);
}
// Log success to this point
syslog(LOG_INFO,"Init OK!");
// Daemon switch
if (config.daemon)
daemonize();
else
if (!config.quiet)
#if defined PWNPAD
printf("Close this window to end scan.\n");
#else
printf("Hit Ctrl+C to end scan.\n");
#endif
// Init result struct
results = (inquiry_info*)malloc(max_results * sizeof(inquiry_info));
// Start scan, be careful with this infinite loop...
for(;;)
{
// Flush results buffer
memset(results, '\0', max_results * sizeof(inquiry_info));
// Scan and return number of results
num_results = hci_inquiry(device, scan_window, max_results, NULL, &results, flags);
// A negative number here means an error during scan
if(num_results < 0)
{
// Increment error count
error_count++;
// Ignore occasional errors on Pwn Plug and OpenWRT
#if !defined PWNPLUG || OPENWRT
// All other platforms, print error and bail out
syslog(LOG_ERR,"Received error from BlueZ!");
printf("Scan failed!\n");
// Check for kernel 3.0.x
if (!strncmp("3.0.",sysinfo.release,4))
{
printf("\n");
printf("-----------------------------------------------------\n");
printf("Device scanning failed, and you are running a 3.0.x\n");
printf("Linux kernel. This failure is probably due to the\n");
printf("following kernel bug:\n");
printf("\n");
printf("http://marc.info/?l=linux-kernel&m=131629118406044\n");
printf("\n");
printf("You will need to upgrade your kernel to at least the\n");
printf("the 3.1 series to continue.\n");
printf("-----------------------------------------------------\n");
}
shut_down(1);
#else
// Exit on back to back errors
if (error_count > 5)
{
printf("Scan failed!\n");
syslog(LOG_ERR,"BlueZ not responding, unrecoverable!");
shut_down(1);
}
// Otherwise, throttle back a bit, might help
sleep(1);
#endif
}
else
{
// Clear error counter
error_count = 0;
}
// Check if we need to reset device cache
if ((cache_index + num_results) >= MAX_DEV)
{
syslog(LOG_INFO,"Resetting device cache...");
memset(dev_cache, 0, sizeof(dev_cache));
cache_index = 0;
}
// Loop through results
for (i = 0; i < num_results; i++)
{
// Return current MAC from struct
ba2str(&(results+i)->bdaddr, addr);
// Compare to device cache
for (ri = 0; ri <= cache_index; ri++)
{
// Determine if device is already logged
if (strcmp (addr, dev_cache[ri].priv_addr) == 0)
{
// This device has been seen before
// Increment seen count, update printed time
dev_cache[ri].seen++;
strcpy(dev_cache[ri].time, get_localtime());
dev_cache[ri].missing_count = 0;
// If we don't have a name, query again
if ((dev_cache[ri].print == 3) && (dev_cache[ri].seen > config.retry_count))
{
syslog(LOG_INFO,"Unable to find name for %s!", addr);
dev_cache[ri].print = 1;
}
else if ((dev_cache[ri].print == 3) && (dev_cache[ri].seen < config.retry_count))
{
// Query name
strcpy(dev_cache[ri].name, namequery(&(results+i)->bdaddr));
// Did we get one?
if (strcmp (dev_cache[ri].name, "VOID") != 0)
{
syslog(LOG_INFO,"Name retry for %s successful!", addr);
// Force print
dev_cache[ri].print = 1;
}
else
syslog(LOG_INFO,"Name retry %i for %s failed!",dev_cache[ri].seen, addr);
}
// Amnesia mode
if (config.amnesia >= 0)
{
// Find current epoch time
epoch = time(NULL);
if ((epoch - dev_cache[ri].epoch) >= (config.amnesia * 60))
{
// Update epoch time
dev_cache[ri].epoch = epoch;
// Set device to print
dev_cache[ri].print = 1;
}
}
// This device is seen before, but has been away
if (strcmp (dev_cache[ri].status, "gone") == 0)
{
dev_cache[ri].print = 1;
strcpy(dev_cache[ri].status, "returned");
}
// Unless we need to get printed, move to next result
if (dev_cache[ri].print != 1)
break;
}
else if (strcmp (dev_cache[ri].addr, "") == 0)
{
// Write new device MAC (visible and internal use)
strcpy(dev_cache[ri].addr, addr);
strcpy(dev_cache[ri].priv_addr, addr);
// Query for name
if (config.getname)
strcpy(dev_cache[ri].name, namequery(&(results+i)->bdaddr));
else
strcpy(dev_cache[ri].name, "IGNORED");
// Get time found
strcpy(dev_cache[ri].time, get_localtime());
dev_cache[ri].epoch = time(NULL);
// Class info
dev_cache[ri].flags = (results+i)->dev_class[2];
dev_cache[ri].major_class = (results+i)->dev_class[1];
dev_cache[ri].minor_class = (results+i)->dev_class[0];
// Init misc variables
dev_cache[ri].seen = 1;
dev_cache[ri].missing_count = 0;
strcpy(dev_cache[ri].status, "new");
// Increment index
cache_index++;
// If we have a device name, get printed
if (strcmp (dev_cache[ri].name, "VOID") != 0)
dev_cache[ri].print = 1;
else
{
// Found with no name.
// Print message to syslog, prevent printing, and move on
syslog(LOG_INFO,"Device %s discovered with no name, will retry", dev_cache[ri].addr);
dev_cache[ri].print = 3;
break;
}
}
// Ready to print?
if (dev_cache[ri].print == 1)
{
// Encode MAC
if (config.encode || config.obfuscate)
{
// Clear buffer
memset(addr_buff, '\0', sizeof(addr_buff));
if (config.obfuscate)
strcpy(addr_buff, mac_obfuscate(dev_cache[ri].priv_addr));
if (config.encode)
strcpy(addr_buff, mac_encode(dev_cache[ri].priv_addr));
// Copy to cache
strcpy(dev_cache[ri].addr, addr_buff);
}
// Print everything to console if verbose is on, optionally friendly class info
if (config.verbose)
{
if (config.friendlyclass)
{
printf("[%s] %s,%s,%s,(%s) - %s\n",\
dev_cache[ri].time, dev_cache[ri].addr,\
dev_cache[ri].name, device_class(dev_cache[ri].major_class,\
dev_cache[ri].minor_class), device_capability(dev_cache[ri].flags), dev_cache[ri].status);
}
else
{
printf("[%s] %s,%s,0x%02x%02x%02x - %s\n",\
dev_cache[ri].time, dev_cache[ri].addr,\
dev_cache[ri].name, dev_cache[ri].flags,\
dev_cache[ri].major_class, dev_cache[ri].minor_class, dev_cache[ri].status);
}
}
if (config.bluelive)
{
// Write result with live function
live_entry(ri);
}
else if (config.bluepropro)
{
// Set output format for BlueProPro
fprintf(outfile,"%s", dev_cache[ri].addr);
fprintf(outfile,",0x%02x%02x%02x", dev_cache[ri].flags,\
dev_cache[ri].major_class, dev_cache[ri].minor_class);
fprintf(outfile,",%s\n", dev_cache[ri].name);
}
else
{
// Flush buffer
memset(outbuffer, 0, sizeof(outbuffer));
// Print time first if enabled
if (config.showtime)
sprintf(outbuffer,"[%s],", dev_cache[ri].time);
// Always output MAC
sprintf(outbuffer+strlen(outbuffer),"%s", dev_cache[ri].addr);
// Optionally output class
if (config.showclass)
sprintf(outbuffer+strlen(outbuffer),",0x%02x%02x%02x", dev_cache[ri].flags,\
dev_cache[ri].major_class, dev_cache[ri].minor_class);
// "Friendly" version of class info
if (config.friendlyclass)
sprintf(outbuffer+strlen(outbuffer),",%s,(%s)",\
device_class(dev_cache[ri].major_class, dev_cache[ri].minor_class),\
device_capability(dev_cache[ri].flags));
// Get manufacturer
if (config.getmanufacturer)
sprintf(outbuffer+strlen(outbuffer),",%s", mac_get_vendor(dev_cache[ri].priv_addr));
// Append the name
if (config.getname)
sprintf(outbuffer+strlen(outbuffer),",%s", dev_cache[ri].name);
// Append the status
if (config.status)
sprintf(outbuffer+strlen(outbuffer)," - %s", dev_cache[ri].status);
// Send buffer, else file. File needs newline
if (config.syslogonly)
syslog(LOG_INFO,"%s", outbuffer);
else if (config.udponly)
{
// Append newline to socket, kind of hacky
sprintf(outbuffer+strlen(outbuffer),"\n");
send_udp_msg(outbuffer);
}
else
fprintf(outfile,"%s\n",outbuffer);
}
dev_cache[ri].print = 0;
break;
}
// If we make it this far, it means we will check next stored device
}
// If there's a file open, write changes
if (outfile != NULL)
fflush(outfile);
}
// Now check if any devices are missing
// Loop through the cache
for (ri = 0; ri < cache_index; ri++)
{
for (i = 0; i <= num_results; i++)
{
// Return current MAC from struct
ba2str(&(results+i)->bdaddr, addr);
// Determine if device still present
if (strcmp (addr, dev_cache[ri].priv_addr) == 0)
{
break;
}
// Device not found.
if (i == num_results)
{
// The device is missing but not marked as gone -> it has just disappeared
if (strcmp(dev_cache[ri].status, "gone") != 0)
{
// Devices aren't present every time. Wait a while before marking it gone
if (dev_cache[ri].missing_count < 10)
{
dev_cache[ri].missing_count++;
}
else
// It's really gone :(
{
strcpy(dev_cache[ri].status,"gone");
// Print to console
if (config.verbose) {
printf("[%s] %s,%s - %s\n",\
dev_cache[ri].time, dev_cache[ri].addr,\
dev_cache[ri].name, dev_cache[ri].status);
}
// Flush buffer
memset(exitbuffer, 0, sizeof(exitbuffer));
// Print time first if enabled
if (config.showtime)
sprintf(exitbuffer,"[%s],", dev_cache[ri].time);
// Always output MAC
sprintf(exitbuffer+strlen(exitbuffer),"%s", dev_cache[ri].addr);
// Append the name
if (config.getname)
sprintf(exitbuffer+strlen(exitbuffer),",%s", dev_cache[ri].name);
// Append the status
if (config.status)
sprintf(exitbuffer+strlen(exitbuffer)," - %s", dev_cache[ri].status);
// Send buffer, else file. File needs newline
if (config.syslogonly)
syslog(LOG_INFO,"%s", exitbuffer);
else if (config.udponly)
{
// Append newline to socket, kind of hacky
sprintf(exitbuffer+strlen(exitbuffer),"\n");
send_udp_msg(exitbuffer);
}
else
fprintf(outfile,"%s\n",exitbuffer);
// If there's a file open, write changes
if (outfile != NULL)
fflush(outfile);
}
}
}
}
}
}
// If we get here, shut down
shut_down(0);
// STFU
return (1);
}
/*
* Add entry to the new inq. If it is not also
* in the old inq, report adds.
*/
void reporter_add(inquiry_info* newthingy) {
char ieeeaddr[18];
int i;
inquiry_info *rstore;
for (i = 0; i < entries1; i++) {
if (0 == bacmp(&((store1+i)->bdaddr),&(newthingy->bdaddr))) {
if (debug) {
ba2str(&(newthingy->bdaddr), ieeeaddr);
printf("= %s %02x %02x %04x ", ieeeaddr,
newthingy->pscan_rep_mode,
newthingy->pscan_mode,
newthingy->clock_offset);
classinfo(newthingy->dev_class);
printf("\n");
}
return;
}
}
/* make storage bigger for bluetooth object if needed */
entries1++;
if ((entries1 * INQUIRY_INFO_SIZE) > capacity1) {
capacity1 = entries1 * INQUIRY_INFO_SIZE;
rstore = realloc(store1, capacity1);
if (rstore == NULL) {
perror("I cannot allocate any more memory for new device, but continuing anyways... :-/");
return;
}
store1 = rstore;
}
/* store in the array */
*(store1+(entries1-1)) = *newthingy;
/* find out if new object is in the old array... C cannot compare structures on its own... */
for (i = 0; i < entries2; i++) {
if (0 == bacmp(&((store2+i)->bdaddr),&(newthingy->bdaddr))) {
if (debug) {
ba2str(&(newthingy->bdaddr), ieeeaddr);
printf("= %s %02x %02x %04x ", ieeeaddr,
newthingy->pscan_rep_mode,
newthingy->pscan_mode,
newthingy->clock_offset);
classinfo(newthingy->dev_class);
printf("\n");
}
return;
}
}
/* it isn't ... report it to chatbot...
* we report the clock offset as it is used to quickly home in on the radio frequency (in the channel hopping sequence)
* that the target is operating on, when we want to send a command to a device.
*
* just for fun when you are running this program in debug mode, you may see the clock offset drift up and down
* slightly, maybe due to doppler effect, interference, etc.
*
* Time ---> (clock offset)
* +---+---+---+---+---+---+---+---+
* R |\ | |/---\ | | /---\ |
* F | \ | / | \---\ | /| |\--|
* | \---/| | | \--/ | | |
* +---+---+---+---+---+---+---+---+
*
*/
/*ba2str(&(newthingy->bdaddr), ieeeaddr);
printf("+ %s %02x %02x %04x ", ieeeaddr,
newthingy->pscan_rep_mode,
newthingy->pscan_mode,
newthingy->clock_offset);
classinfo(newthingy->dev_class);
printf("\n");
*/
}
static int ble_scan(int device_desc, ble_discovered_device_t discovered_device_cb, int timeout) {
struct hci_filter old_options;
socklen_t slen = sizeof(old_options);
struct hci_filter new_options;
int len;
unsigned char buffer[HCI_MAX_EVENT_SIZE];
evt_le_meta_event* meta = (evt_le_meta_event*)(buffer + HCI_EVENT_HDR_SIZE + 1);
le_advertising_info* info;
struct timeval wait;
fd_set read_set;
char addr[18];
if (getsockopt(device_desc, SOL_HCI, HCI_FILTER, &old_options, &slen) < 0) {
fprintf(stderr, "ERROR: Could not get socket options.\n");
return 1;
}
hci_filter_clear(&new_options);
hci_filter_set_ptype(HCI_EVENT_PKT, &new_options);
hci_filter_set_event(EVT_LE_META_EVENT, &new_options);
if (setsockopt(device_desc, SOL_HCI, HCI_FILTER,
&new_options, sizeof(new_options)) < 0) {
fprintf(stderr, "ERROR: Could not set socket options.\n");
return 1;
}
wait.tv_sec = timeout;
int ts = time(NULL);
while(1) {
FD_ZERO(&read_set);
FD_SET(device_desc, &read_set);
int err = select(FD_SETSIZE, &read_set, NULL, NULL, &wait);
if (err <= 0)
break;
len = read(device_desc, buffer, sizeof(buffer));
if (meta->subevent != 0x02 || (uint8_t)buffer[BLE_EVENT_TYPE] != BLE_SCAN_RESPONSE)
continue;
info = (le_advertising_info*) (meta->data + 1);
ba2str(&info->bdaddr, addr);
char* name = parse_name(info->data, info->length);
discovered_device_cb(addr, name);
if (name) {
free(name);
}
int elapsed = time(NULL) - ts;
if (elapsed >= timeout)
break;
wait.tv_sec = timeout - elapsed;
}
setsockopt(device_desc, SOL_HCI, HCI_FILTER, &old_options, sizeof(old_options));
return 0;
}
void peisk_bluetoothAcceptConnections() {
int i,len;
PeisBluetoothAdaptor *adaptor;
socklen_t opt = sizeof(struct sockaddr_l2);
char str[256];
PeisConnectMessage message;
PeisConnection *connection;
for(i=0;i<peisk_nBluetoothAdaptors;i++) {
adaptor = &peisk_bluetoothAdaptors[i];
if(adaptor->incomming.mode == 0) {
adaptor->incomming.socket = accept(adaptor->listenSocket,(struct sockaddr*) &adaptor->incomming.remoteAddr, &opt);
if(adaptor->incomming.socket == -1) continue;
printf("Setting MTU of incomming socket\n");
peisk_bluetooth_setMTU(adaptor->incomming.socket);
#ifdef OLD
struct l2cap_options opts;
struct l2cap_conninfo conn;
socklen_t optlen;
printf("Setting MTU of incomming socket\n");
memset(&opts, 0, sizeof(opts));
optlen = sizeof(opts);
if (getsockopt(adaptor->incomming.socket, SOL_L2CAP, L2CAP_OPTIONS, &opts, &optlen) < 0) {
perror("Can't get default L2CAP options");
}
/* Set new options */
printf("Old L2CAP MTU: %d %d\n",opts.omtu,opts.imtu);
opts.omtu = PEISK_MAX_PACKAGE_SIZE;
opts.imtu = PEISK_MAX_PACKAGE_SIZE;
/* if (rfcmode > 0)
opts.mode = rfcmode;*/
#endif
ba2str(&adaptor->incomming.remoteAddr.l2_bdaddr,str);
printf("Incomming connection from: %s\n",str);
if(fcntl(adaptor->incomming.socket,F_SETFL,O_NONBLOCK) != 0) {
perror("Failed to set incomming socket nonblocking\n");
}
adaptor->incomming.timeout = peisk_timeNow + 5.0;
adaptor->incomming.mode = 1;
}
if(adaptor->incomming.mode == 1) {
len = read(adaptor->incomming.socket,(void*)&message,sizeof(message));
if(len == sizeof(message)) {
/* Create connection structure to use */
printf("Received connection message:\n"); peisk_hexDump(&message,len); printf("\n");
connection = peisk_newConnection();
if(!connection) {
fprintf(stderr,"peisk::error - failed to create connection structure\n");
return;
}
if(peisk_verifyConnectMessage(connection,&message) != 0) {
printf("do not accept him\n");
peisk_abortConnect(connection);
close(adaptor->incomming.socket);
adaptor->incomming.mode = 0;
continue;
}
connection->type = eBluetoothConnection;
connection->connection.bluetooth.socket = adaptor->incomming.socket;
connection->connection.bluetooth.adaptor = adaptor;
printf("connection %x accepted\n",(int)connection);
/* Let P2P layer handle this connection */
peisk_incommingConnectFinished(connection,&message);
if(1 || peisk_printLevel & PEISK_PRINT_CONNECTIONS)
printf("peisk: accepted new incomming BLUETOOTH connection to %d with index: %d, flags=%d\n",
message.id,connection->id,message.flags);
adaptor->incomming.mode = 0;
} else if(len > 0) {
fprintf(stderr,"Warning, received incorrect length on incomming bluetooth connection\n");
close(adaptor->incomming.socket);
adaptor->incomming.mode = 0;
continue;
}
}
if(adaptor->incomming.mode == 1 && adaptor->incomming.timeout < peisk_timeNow) {
/* peisk_abortConnect(adaptor->outgoing.connection); ?? */
peisk_freeConnection(connection);
close(adaptor->incomming.socket);
adaptor->incomming.mode = 0;
}
}
}
static int do_connect(const char *svr)
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, opt;
if (uuid != 0x0000)
channel = get_channel(svr, uuid);
if (channel == 0) {
syslog(LOG_ERR, "Can't get channel number");
return -1;
}
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
return -1;
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
if (bacmp(&auto_bdaddr, BDADDR_ANY))
bacpy(&addr.rc_bdaddr, &auto_bdaddr);
else
bacpy(&addr.rc_bdaddr, &bdaddr);
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(sk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(sk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
goto error;
}
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encr)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Connect to remote device */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
str2ba(svr, &addr.rc_bdaddr);
addr.rc_channel = channel;
if (connect(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't connect: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(sk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//goto error;
}
if (priority > 0 && setsockopt(sk, SOL_SOCKET, SO_PRIORITY, &priority,
sizeof(priority)) < 0) {
syslog(LOG_ERR, "Can't set socket priority: %s (%d)",
strerror(errno), errno);
goto error;
}
if (getsockopt(sk, SOL_SOCKET, SO_PRIORITY, &opt, &optlen) < 0) {
syslog(LOG_ERR, "Can't get socket priority: %s (%d)",
strerror(errno), errno);
goto error;
}
syslog(LOG_INFO, "Connected [handle %d, class 0x%02x%02x%02x, "
"priority %d]", conn.hci_handle, conn.dev_class[2],
conn.dev_class[1], conn.dev_class[0], opt);
return sk;
error:
close(sk);
return -1;
}
static void do_listen(void (*handler)(int sk))
{
struct sockaddr_rc addr;
struct rfcomm_conninfo conn;
socklen_t optlen;
int sk, nsk, opt;
char ba[18];
/* Create socket */
sk = socket(PF_BLUETOOTH, socktype, BTPROTO_RFCOMM);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
exit(1);
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.rc_family = AF_BLUETOOTH;
bacpy(&addr.rc_bdaddr, &bdaddr);
addr.rc_channel = channel;
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Set link mode */
opt = 0;
if (master)
opt |= RFCOMM_LM_MASTER;
if (auth)
opt |= RFCOMM_LM_AUTH;
if (encr)
opt |= RFCOMM_LM_ENCRYPT;
if (secure)
opt |= RFCOMM_LM_SECURE;
if (opt && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't set RFCOMM link mode: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Enable deferred setup */
opt = defer_setup;
if (opt && setsockopt(sk, SOL_BLUETOOTH, BT_DEFER_SETUP,
&opt, sizeof(opt)) < 0) {
syslog(LOG_ERR, "Can't enable deferred setup : %s (%d)",
strerror(errno), errno);
goto error;
}
/* Listen for connections */
if (listen(sk, 10)) {
syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Check for socket address */
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
if (getsockname(sk, (struct sockaddr *) &addr, &optlen) < 0) {
syslog(LOG_ERR, "Can't get socket name: %s (%d)",
strerror(errno), errno);
goto error;
}
channel = addr.rc_channel;
syslog(LOG_INFO, "Waiting for connection on channel %d ...", channel);
while (1) {
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
if (nsk < 0) {
syslog(LOG_ERR,"Accept failed: %s (%d)",
strerror(errno), errno);
goto error;
}
if (fork()) {
/* Parent */
close(nsk);
continue;
}
/* Child */
close(sk);
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(nsk, SOL_RFCOMM, RFCOMM_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get RFCOMM connection information: %s (%d)",
strerror(errno), errno);
//close(nsk);
//goto error;
}
if (priority > 0 && setsockopt(sk, SOL_SOCKET, SO_PRIORITY,
&priority, sizeof(priority)) < 0) {
syslog(LOG_ERR, "Can't set socket priority: %s (%d)",
strerror(errno), errno);
close(nsk);
goto error;
}
optlen = sizeof(priority);
if (getsockopt(nsk, SOL_SOCKET, SO_PRIORITY, &opt, &optlen) < 0) {
syslog(LOG_ERR, "Can't get socket priority: %s (%d)",
strerror(errno), errno);
goto error;
}
ba2str(&addr.rc_bdaddr, ba);
syslog(LOG_INFO, "Connect from %s [handle %d, "
"class 0x%02x%02x%02x, priority %d]",
ba, conn.hci_handle, conn.dev_class[2],
conn.dev_class[1], conn.dev_class[0], opt);
#if 0
/* Enable SO_TIMESTAMP */
if (timestamp) {
int t = 1;
if (setsockopt(nsk, SOL_SOCKET, SO_TIMESTAMP, &t, sizeof(t)) < 0) {
syslog(LOG_ERR, "Can't enable SO_TIMESTAMP: %s (%d)",
strerror(errno), errno);
goto error;
}
}
#endif
/* Enable SO_LINGER */
if (linger) {
struct linger l = { .l_onoff = 1, .l_linger = linger };
if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) {
syslog(LOG_ERR, "Can't enable SO_LINGER: %s (%d)",
strerror(errno), errno);
close(nsk);
goto error;
}
}
/* Handle deferred setup */
if (defer_setup) {
syslog(LOG_INFO, "Waiting for %d seconds",
abs(defer_setup) - 1);
sleep(abs(defer_setup) - 1);
if (defer_setup < 0) {
close(nsk);
goto error;
}
}
handler(nsk);
syslog(LOG_INFO, "Disconnect: %m");
exit(0);
}
return;
error:
close(sk);
exit(1);
}
static void dump_mode(int sk)
{
int len;
syslog(LOG_INFO, "Receiving ...");
while ((len = read(sk, buf, data_size)) > 0)
syslog(LOG_INFO, "Received %d bytes", len);
}
static void save_mode(int sk)
{
int len, ret;
char *b;
b = malloc(data_size);
if (!b) {
syslog(LOG_ERR, "Failed to open file to save recv data");
return;
}
syslog(LOG_INFO, "Receiving ...");
while ((len = read(sk, b, data_size)) > 0) {
ret = write(save_fd, b, len);
if (ret < 0)
goto done;
}
done:
free(b);
}
int main(int argc, char *argv[])
{
GError *err = NULL;
bdaddr_t src, dst;
int opt;
char bdastr[18];
hci_devba(0, &src);
bacpy(&dst, BDADDR_ANY);
mloop = g_main_loop_new(NULL, FALSE);
if (!mloop) {
printf("Cannot create main loop\n");
exit(1);
}
while ((opt = getopt_long(argc, argv, "+i:c:C:D:e:f:dghunab",
main_options, NULL)) != EOF) {
switch (opt) {
case 'i':
if (!strncmp(optarg, "hci", 3))
hci_devba(atoi(optarg + 3), &src);
else
str2ba(optarg, &src);
break;
case 'c':
control_mode = MODE_CONNECT;
str2ba(optarg, &dst);
break;
case 'd':
data_mode = MODE_CONNECT;
break;
case 'a':
mdl_disconnect = TRUE;
break;
case 'b':
mcl_disconnect = TRUE;
break;
case 'e':
mcl_disconnect_timeout = atoi(optarg);
break;
case 'f':
mdl_disconnect_timeout = atoi(optarg);
break;
case 'g':
send_synccap_req = TRUE;
break;
case 'u':
mdl_conn_req_result = MCAP_RESOURCE_UNAVAILABLE;
break;
case 'n':
no_close = TRUE;
break;
case 'C':
ccpsm = atoi(optarg);
break;
case 'D':
dcpsm = atoi(optarg);
break;
case 'h':
default:
usage();
exit(0);
}
}
mcap = mcap_create_instance(&src, BT_IO_SEC_MEDIUM, ccpsm, dcpsm,
mcl_connected, mcl_reconnected,
mcl_disconnected, mcl_uncached,
NULL, /* CSP is not used right now */
NULL, &err);
if (!mcap) {
printf("MCAP instance creation failed %s\n", err->message);
g_error_free(err);
exit(1);
}
mcap_enable_csp(mcap);
switch (control_mode) {
case MODE_CONNECT:
ba2str(&dst, bdastr);
printf("Connecting to %s\n", bdastr);
mcap_create_mcl(mcap, &dst, ccpsm, create_mcl_cb, NULL, NULL,
&err);
if (err) {
printf("MCAP create error %s\n", err->message);
g_error_free(err);
exit(1);
}
break;
case MODE_LISTEN:
printf("Listening for control channel connection\n");
break;
case MODE_NONE:
default:
goto done;
}
g_main_loop_run(mloop);
done:
printf("Done\n");
if (mcap)
mcap_instance_unref(mcap);
g_main_loop_unref(mloop);
return 0;
}
static void stack_internal_callback(int fd, uint32_t events, void *user_data)
{
unsigned char buf[HCI_MAX_FRAME_SIZE];
unsigned char control[32];
struct msghdr msg;
struct iovec iov;
struct cmsghdr *cmsg;
ssize_t len;
hci_event_hdr *eh;
evt_stack_internal *si;
evt_si_device *sd;
struct timeval *tv = NULL;
struct timeval ctv;
uint8_t type = 0xff, bus = 0xff;
char str[18], name[8] = "";
bdaddr_t bdaddr;
bacpy(&bdaddr, BDADDR_ANY);
if (events & (EPOLLERR | EPOLLHUP)) {
mainloop_remove_fd(fd);
return;
}
iov.iov_base = buf;
iov.iov_len = sizeof(buf);
memset(&msg, 0, sizeof(msg));
msg.msg_iov = &iov;
msg.msg_iovlen = 1;
msg.msg_control = control;
msg.msg_controllen = sizeof(control);
len = recvmsg(fd, &msg, MSG_DONTWAIT);
if (len < 0)
return;
for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
cmsg = CMSG_NXTHDR(&msg, cmsg)) {
if (cmsg->cmsg_level != SOL_HCI)
continue;
switch (cmsg->cmsg_type) {
case HCI_CMSG_TSTAMP:
memcpy(&ctv, CMSG_DATA(cmsg), sizeof(ctv));
tv = &ctv;
break;
}
}
if (len < 1 + HCI_EVENT_HDR_SIZE + EVT_STACK_INTERNAL_SIZE +
EVT_SI_DEVICE_SIZE)
return;
if (buf[0] != HCI_EVENT_PKT)
return;
eh = (hci_event_hdr *) (buf + 1);
if (eh->evt != EVT_STACK_INTERNAL)
return;
si = (evt_stack_internal *) (buf + 1 + HCI_EVENT_HDR_SIZE);
if (si->type != EVT_SI_DEVICE)
return;
sd = (evt_si_device *) &si->data;
switch (sd->event) {
case HCI_DEV_REG:
device_info(fd, sd->dev_id, &type, &bus, &bdaddr, name);
ba2str(&bdaddr, str);
packet_new_index(tv, sd->dev_id, str, type, bus, name);
open_device(sd->dev_id);
break;
case HCI_DEV_UNREG:
ba2str(&bdaddr, str);
packet_del_index(tv, sd->dev_id, str);
break;
}
}
static int hidp_add_connection(const struct input_device *idev,
const struct input_conn *iconn)
{
struct hidp_connadd_req *req;
struct fake_hid *fake_hid;
struct fake_input *fake;
sdp_record_t *rec;
char src_addr[18], dst_addr[18];
int err;
req = g_new0(struct hidp_connadd_req, 1);
req->ctrl_sock = g_io_channel_unix_get_fd(iconn->ctrl_io);
req->intr_sock = g_io_channel_unix_get_fd(iconn->intr_io);
req->flags = 0;
req->idle_to = iconn->timeout;
ba2str(&idev->src, src_addr);
ba2str(&idev->dst, dst_addr);
rec = fetch_record(src_addr, dst_addr, idev->handle);
if (!rec) {
error("Rejected connection from unknown device %s", dst_addr);
err = -EPERM;
goto cleanup;
}
extract_hid_record(rec, req);
sdp_record_free(rec);
read_device_id(src_addr, dst_addr, NULL,
&req->vendor, &req->product, &req->version);
fake_hid = get_fake_hid(req->vendor, req->product);
if (fake_hid) {
fake = g_new0(struct fake_input, 1);
fake->connect = fake_hid_connect;
fake->disconnect = fake_hid_disconnect;
fake->priv = fake_hid;
err = fake_hid_connadd(fake, iconn->intr_io, fake_hid);
goto cleanup;
}
if (idev->name)
strncpy(req->name, idev->name, sizeof(req->name) - 1);
/* Encryption is mandatory for keyboards */
if (req->subclass & 0x40) {
err = bt_acl_encrypt(&idev->src, &idev->dst, encrypt_completed, req);
if (err == 0) {
/* Waiting async encryption */
return 0;
} else if (err != -EALREADY) {
error("bt_acl_encrypt(): %s(%d)", strerror(-err), -err);
goto cleanup;
}
}
err = ioctl_connadd(req);
cleanup:
if (req->rd_data)
free(req->rd_data);
g_free(req);
return err;
}
int main(int argc, char **argv)
{
inquiry_info *ii = NULL;
int max_rsp, num_rsp;
int dev_id, sock, len, flags,loglines,socket_fd;
int i;
int first_check_flag=0;
char addr[19] = { 0 };
char name[248] = { 0 };
char current_time[50];
info data;
pthread_t pid;
pthread_attr_t attr;
pthread_mutex_t mutex;
getEth0Mac(data.mac);
pthread_create(&pid,NULL,relay2server,NULL);
InitQueue(& infoQ);
InitQueue(& unSendQ);
sleep(2);
dev_id = hci_get_route(NULL);
sock = hci_open_dev( dev_id );
if (dev_id < 0 || sock < 0) {
perror("opening socket");
exit(1);
}
// len = 7;
max_rsp = 255;
loglines = 0;
flags = IREQ_CACHE_FLUSH;
ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
time_t now;
struct tm *timenow;
#ifdef LOGFILE_ON
config(&maxLoglines,&len,&logname);
FILE *logFile;
logFile=fopen(logname,"w");
#endif
while (1){
time(&now);
timenow = localtime(&now);
sprintf(current_time, "%d%02d%02d%02d%02d%02d",
timenow->tm_year+1900, timenow->tm_mon+1, timenow->tm_mday, timenow->tm_hour, timenow->tm_min, timenow->tm_sec);
#ifdef TERMINAL_ON
printf("inquiry starts: %s",asctime(timenow));
#endif
#ifdef LOGFILE_ON
fprintf(logFile,"inquiry starts: %s",asctime(timenow));
#endif
num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
if( num_rsp < 0 ) perror("hci_inquiry");
else
{
if(first_check_flag ==0)
{
first_check_flag = 1;
mkfifo("fifo",0777);
}
}
for (i = 0; i < num_rsp; i++) {
ba2str(&(ii+i)->bdaddr, addr);
memcpy(data.addr,addr,sizeof(addr));
//memcpy(data.time,current_time,sizeof(current_time));
memcpy(data.time,asctime(timenow),sizeof(data.time));
int res = EnQueue(infoQ, data);
if(!res)
{
printf("The infoQueue is full \n");
}
#ifdef TERMINAL_ON
printf("%s %s\n", addr, name);
#endif
#ifdef LOGFILE_ON
loglines++;
fprintf(logFile, "%s %s\n", addr, name);
#endif
}
#ifdef TERMINAL_ON
printf("%d devices found.\n",num_rsp);
printf("---------------------------------------\n");
fflush(stdout);
#endif
#ifdef LOGFILE_ON
loglines++;
fprintf(logFile,"%d devices found.\n",num_rsp);
fprintf(logFile,"---------------------------------------\n");
fflush(logFile);
logHandler(&loglines,current_time,&logFile);
#endif
}
#ifdef LOGFILE_ON
fclose(logFile);
#endif
free( ii );
close( sock );
return 0;
}
int main(int argc, char **argv)
{
printf("starting...\n");
inquiry_info *ii = NULL;
int max_rsp, num_rsp;
int dev_id, sock, len, flags;
char addr[19] = { 0 };
char name[248] = { 0 };
printf("looking for device...");
dev_id = hci_get_route(NULL);
if (dev_id < 0) {
perror("locating device");
exit(1);
}
printf("found!\n");
printf("opening socket...");
sock = hci_open_dev(dev_id);
if (sock < 0) {
perror("opening socket");
exit(1);
}
printf("open!\n");
len = 8;
max_rsp = 255;
flags = IREQ_CACHE_FLUSH;
unsigned int requiredMemorySize = max_rsp * sizeof(inquiry_info);
ii = (inquiry_info*)malloc(requiredMemorySize);
printf("looking for devices....\n");
num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
if (num_rsp < 0) perror ("hci_inquiry");
printf("found %d devices\n", num_rsp);
for (int i = 0; i < num_rsp; i++) {
ba2str(&(ii+i)->bdaddr, addr);
memset(name, 0, sizeof(name));
int remoteNameFound = hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), name, 0);
if (remoteNameFound < 0) {
strcpy(name, "[unknown]");
}
printf("%d\t%s\t%s\n", &(ii+i)->bdaddr, addr, name);
}
printf("done... closing...\n");
free(ii);
close(sock);
return 0;
}
static void a2dp_discovery_complete(struct avdtp *session, GSList *seps,
struct avdtp_error *err,
void *user_data)
{
struct unix_client *client = user_data;
char buf[BT_SUGGESTED_BUFFER_SIZE];
struct bt_get_capabilities_rsp *rsp = (void *) buf;
struct a2dp_data *a2dp = &client->d.a2dp;
GSList *l;
if (!g_slist_find(clients, client)) {
DBG("Client disconnected during discovery");
return;
}
if (err)
goto failed;
memset(buf, 0, sizeof(buf));
client->req_id = 0;
rsp->h.type = BT_RESPONSE;
rsp->h.name = BT_GET_CAPABILITIES;
rsp->h.length = sizeof(*rsp);
ba2str(&client->dev->src, rsp->source);
ba2str(&client->dev->dst, rsp->destination);
strncpy(rsp->object, client->dev->path, sizeof(rsp->object));
for (l = seps; l; l = g_slist_next(l)) {
struct avdtp_remote_sep *rsep = l->data;
struct a2dp_sep *sep;
struct avdtp_service_capability *cap;
struct avdtp_stream *stream;
uint8_t type, seid, configured = 0, lock = 0;
GSList *cl;
type = avdtp_get_type(rsep);
if (type != AVDTP_SEP_TYPE_SINK &&
type != AVDTP_SEP_TYPE_SOURCE)
continue;
cap = avdtp_get_codec(rsep);
if (cap->category != AVDTP_MEDIA_CODEC)
continue;
seid = avdtp_get_seid(rsep);
if (client->seid != 0 && client->seid != seid)
continue;
stream = avdtp_get_stream(rsep);
if (stream) {
configured = 1;
if (client->seid == seid)
cap = avdtp_stream_get_codec(stream);
}
for (cl = clients; cl; cl = cl->next) {
struct unix_client *c = cl->data;
struct a2dp_data *ca2dp = &c->d.a2dp;
if (ca2dp->session == session && c->seid == seid) {
lock = c->lock;
break;
}
}
sep = a2dp_get_sep(session, stream);
if (sep && a2dp_sep_get_lock(sep))
lock = BT_WRITE_LOCK;
a2dp_append_codec(rsp, cap, seid, type, configured, lock);
}
unix_ipc_sendmsg(client, &rsp->h);
return;
failed:
error("discovery failed");
unix_ipc_error(client, BT_GET_CAPABILITIES, EIO);
if (a2dp->sep) {
a2dp_sep_unlock(a2dp->sep, a2dp->session);
a2dp->sep = NULL;
}
avdtp_unref(a2dp->session);
a2dp->session = NULL;
a2dp->stream = NULL;
}
void WiimoteScanner::FindWiimotes(std::vector<Wiimote*>& found_wiimotes, Wiimote*& found_board)
{
// supposedly 1.28 seconds
int const wait_len = 1;
int const max_infos = 255;
inquiry_info scan_infos[max_infos] = {};
auto* scan_infos_ptr = scan_infos;
found_board = nullptr;
// Scan for Bluetooth devices
int const found_devices =
hci_inquiry(device_id, wait_len, max_infos, nullptr, &scan_infos_ptr, IREQ_CACHE_FLUSH);
if (found_devices < 0)
{
ERROR_LOG(WIIMOTE, "Error searching for Bluetooth devices.");
return;
}
DEBUG_LOG(WIIMOTE, "Found %i Bluetooth device(s).", found_devices);
// Display discovered devices
for (int i = 0; i < found_devices; ++i)
{
ERROR_LOG(WIIMOTE, "found a device...");
// BT names are a maximum of 248 bytes apparently
char name[255] = {};
if (hci_read_remote_name(device_sock, &scan_infos[i].bdaddr, sizeof(name), name, 1000) < 0)
{
ERROR_LOG(WIIMOTE, "name request failed");
continue;
}
ERROR_LOG(WIIMOTE, "device name %s", name);
if (IsValidBluetoothName(name))
{
bool new_wiimote = true;
// TODO: do this
// Determine if this Wiimote has already been found.
// for (int j = 0; j < MAX_WIIMOTES && new_wiimote; ++j)
//{
// if (wm[j] && bacmp(&scan_infos[i].bdaddr,&wm[j]->bdaddr) == 0)
// new_wiimote = false;
//}
if (new_wiimote)
{
// Found a new device
char bdaddr_str[18] = {};
ba2str(&scan_infos[i].bdaddr, bdaddr_str);
Wiimote* wm = new WiimoteLinux(scan_infos[i].bdaddr);
if (IsBalanceBoardName(name))
{
found_board = wm;
NOTICE_LOG(WIIMOTE, "Found balance board (%s).", bdaddr_str);
}
else
{
found_wiimotes.push_back(wm);
NOTICE_LOG(WIIMOTE, "Found Wiimote (%s).", bdaddr_str);
}
}
}
}
}
void process_data(uint8_t *data, size_t data_len, le_advertising_info *info)
{
// printw("Test: %p and %d\n", data, data_len);
if(data[0] == EIR_NAME_SHORT )//|| data[0] == EIR_NAME_COMPLETE)
{
size_t name_len = data_len - 1;
uint16_t check_manu;
char *name = malloc(name_len + 1);
memset(name, 0, name_len + 1);
memcpy(name, &data[2], name_len);
char addr[18];
ba2str(&info->bdaddr, addr);
printw("addr=%s name=%s\n", addr, name);
free(name);
}
else if(data[0] == EIR_FLAGS)
{
printw("Flag type: len=%d\n", data_len);
int i;
for(i=1; i<data_len; i++)
{
printw("\tFlag data: 0x%0X\n", data[i]);
}
}
else if(data[0] == EIR_MANUFACTURE_SPECIFIC || data[0] == EIR_COMPLETE_SERVICE_16BIT ||
data[0] == EIR_APPEARANCE || data[0] == EIR_POWER_LEVEL || data[0] == EIR_NAME_COMPLETE ||
data[0] == EIR_SLAVE_CONNECT || data[0] == EIR_INCOMPLETE_SERVICE_16BIT)
{
// printf("Manufacture specific type: len=%d\n", data_len);
// TODO int company_id = data[current_index + 2]
int i;
// if(data[5] == 0x5c)
// {
// printf("manufacture: BELKIN \n");
// printf("status: ");
// switch(data[6])
// {
// case 0x00:
// {
// printf("unconfig \n");
// break;
// }
// case 0x01:
// {
// printf("config \n");
// break;
// }
// default:
// break;
// }
// }
for(i=1; i<data_len; i++)
{
printw("\tData: 0x%0X\n", data[i]);
}
}
else
{
printw("Unknown type: type=%X\n", data[0]);
}
}
int wiiuse_os_find(struct wiimote_t** wm, int max_wiimotes, int timeout) {
int device_id;
int device_sock;
inquiry_info scan_info_arr[128];
inquiry_info* scan_info = scan_info_arr;
int found_devices;
int found_wiimotes;
int i = 0;
/* reset all wiimote bluetooth device addresses */
for (found_wiimotes = 0; found_wiimotes < max_wiimotes; ++found_wiimotes) {
/* bacpy(&(wm[found_wiimotes]->bdaddr), BDADDR_ANY); */
memset(&(wm[found_wiimotes]->bdaddr), 0, sizeof(bdaddr_t));
}
found_wiimotes = 0;
/* get the id of the first bluetooth device. */
device_id = hci_get_route(NULL);
if (device_id < 0) {
if (errno == ENODEV) {
WIIUSE_ERROR("Could not detect a Bluetooth adapter!");
} else {
perror("hci_get_route");
}
return 0;
}
/* create a socket to the device */
device_sock = hci_open_dev(device_id);
if (device_sock < 0) {
perror("hci_open_dev");
return 0;
}
memset(&scan_info_arr, 0, sizeof(scan_info_arr));
/* scan for bluetooth devices for 'timeout' seconds */
found_devices = hci_inquiry(device_id, timeout, 128, NULL, &scan_info, IREQ_CACHE_FLUSH);
if (found_devices < 0) {
perror("hci_inquiry");
close(device_sock);
return 0;
}
WIIUSE_INFO("Found %i bluetooth device(s).", found_devices);
/* display discovered devices */
for (i = 0; (i < found_devices) && (found_wiimotes < max_wiimotes); ++i) {
bool is_wiimote_regular = (scan_info[i].dev_class[0] == WM_DEV_CLASS_0) &&
(scan_info[i].dev_class[1] == WM_DEV_CLASS_1) &&
(scan_info[i].dev_class[2] == WM_DEV_CLASS_2);
bool is_wiimote_plus = (scan_info[i].dev_class[0] == WM_PLUS_DEV_CLASS_0) &&
(scan_info[i].dev_class[1] == WM_PLUS_DEV_CLASS_1) &&
(scan_info[i].dev_class[2] == WM_PLUS_DEV_CLASS_2);
if (is_wiimote_regular || is_wiimote_plus) {
/* found a device */
ba2str(&scan_info[i].bdaddr, wm[found_wiimotes]->bdaddr_str);
const char* str_type;
if(is_wiimote_regular)
{
wm[found_wiimotes]->type = WIIUSE_WIIMOTE_REGULAR;
str_type = " (regular wiimote)";
}
else if(is_wiimote_plus)
{
wm[found_wiimotes]->type = WIIUSE_WIIMOTE_MOTION_PLUS_INSIDE;
str_type = " (motion plus inside)";
}
WIIUSE_INFO("Found wiimote (type: %s) (%s) [id %i].", str_type, wm[found_wiimotes]->bdaddr_str, wm[found_wiimotes]->unid);
wm[found_wiimotes]->bdaddr = scan_info[i].bdaddr;
WIIMOTE_ENABLE_STATE(wm[found_wiimotes], WIIMOTE_STATE_DEV_FOUND);
++found_wiimotes;
}
}
close(device_sock);
return found_wiimotes;
}
static void do_listen(void (*handler)(int sk))
{
struct sockaddr_sco addr;
struct sco_conninfo conn;
socklen_t optlen;
int sk, nsk;
char ba[18];
/* Create socket */
sk = socket(PF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_SCO);
if (sk < 0) {
syslog(LOG_ERR, "Can't create socket: %s (%d)",
strerror(errno), errno);
exit(1);
}
/* Bind to local address */
memset(&addr, 0, sizeof(addr));
addr.sco_family = AF_BLUETOOTH;
bacpy(&addr.sco_bdaddr, &bdaddr);
if (bind(sk, (struct sockaddr *) &addr, sizeof(addr)) < 0) {
syslog(LOG_ERR, "Can't bind socket: %s (%d)",
strerror(errno), errno);
goto error;
}
/* Listen for connections */
if (listen(sk, 10)) {
syslog(LOG_ERR,"Can not listen on the socket: %s (%d)",
strerror(errno), errno);
goto error;
}
syslog(LOG_INFO,"Waiting for connection ...");
while (1) {
memset(&addr, 0, sizeof(addr));
optlen = sizeof(addr);
nsk = accept(sk, (struct sockaddr *) &addr, &optlen);
if (nsk < 0) {
syslog(LOG_ERR,"Accept failed: %s (%d)",
strerror(errno), errno);
goto error;
}
if (fork()) {
/* Parent */
close(nsk);
continue;
}
/* Child */
close(sk);
/* Get connection information */
memset(&conn, 0, sizeof(conn));
optlen = sizeof(conn);
if (getsockopt(nsk, SOL_SCO, SCO_CONNINFO, &conn, &optlen) < 0) {
syslog(LOG_ERR, "Can't get SCO connection information: %s (%d)",
strerror(errno), errno);
close(nsk);
goto error;
}
ba2str(&addr.sco_bdaddr, ba);
syslog(LOG_INFO, "Connect from %s [handle %d, class 0x%02x%02x%02x]",
ba, conn.hci_handle,
conn.dev_class[2], conn.dev_class[1], conn.dev_class[0]);
handler(nsk);
syslog(LOG_INFO, "Disconnect");
exit(0);
}
return;
error:
close(sk);
exit(1);
}
//TODO: change to bluetooth address to specified
int main(int argc, char **argv) {
//程序员面试宝典
struct sockaddr_l2 local_addr, remote_addr;
//hci_open_route
char buf[60000] = {0};
int listensock, clientsock, err = 0, retval = 0;
unsigned int optlen = sizeof(remote_addr);
ssize_t count = 0;
int i;
listensock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (listensock < 0)
return listensock;
if (set_l2cap_mtu(listensock, 65000) != 0) {
perror("set_l2cap_mtu");
goto failed;
}
//Bind to local adapter
memset(&local_addr, 0, sizeof(local_addr));
local_addr.l2_family = AF_BLUETOOTH;
local_addr.l2_bdaddr = *BDADDR_ANY;
local_addr.l2_psm = htobs(0x1001);
if (bind(listensock, (struct sockaddr *)&local_addr, sizeof(local_addr)))
goto failed;
if (listen(listensock, 1) < 0)
goto failed;
memset(&remote_addr, 0, sizeof(remote_addr));
for ( ; ; ) {
clientsock = accept(listensock, (struct sockaddr *)&remote_addr, &optlen);
if (clientsock < 0) {
perror("accpet error");
//syslog(LOG_ERR, "ac")
goto failed;
}
// if(set_flush_timeout(&remote_addr.l2_bdaddr, 10)) {
// perror("set flush timeout");
// goto failed1;
// }
i = 0;
ba2str(&remote_addr.l2_bdaddr, buf);
fprintf(stderr, "accepted connection from %s\n", buf);
do {
count = recv(clientsock, buf, sizeof(buf), 0);
if(count > 0) {
printf("received [%d]:%d\n", count, i);
} else if(count == 0){
printf("client disconnect\n");
} else {
perror("recv error");
}
//count = send(clientsock, "yes you are the best!", 20, 0);
i++;
} while (count > 0);
failed1:
close(clientsock);
}
//set MTU(maximum transfer unit)optlen
//set no retranssimition: set auto flush number to a small value
//set encry
failed:
err = errno;
close(listensock);
errno = err;
return -1;
}
// Scan for available bluetooth devices
int scan(char * log_file, char * filename)
{
inquiry_info *ii = NULL;
int max_rsp, num_rsp;
int dev_id, sock, len, flags;
int i;
int t_count;
char addr[19] = { 0 };
dev_id = hci_get_route(NULL);
sock = hci_open_dev( dev_id );
// Error with opening socket
if (dev_id < 0 || sock < 0) {
perror("error: opening socket\n");
return 1;
}
len = 8;
max_rsp = 255;
flags = IREQ_CACHE_FLUSH;
ii = (inquiry_info*)malloc(max_rsp * sizeof(inquiry_info));
num_rsp = hci_inquiry(dev_id, len, max_rsp, NULL, &ii, flags);
// Check for any devices found
if( num_rsp < 0 ) {
return 1;
}
// Cycle through all the addresses found
for (i = 0; i < num_rsp; i++) {
char name[248] = { 0 };
ba2str(&(ii+i)->bdaddr, addr);
memset(name, 0, sizeof(name));
// Check for device name
if (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), name, 0) < 0)
strcpy(name, "[unknown]");
// while (hci_read_remote_name(sock, &(ii+i)->bdaddr, sizeof(name), name, 0) < 0) {
// // Requesting device name
// }
int num = 0;
int matchfound = 0;
// Check to see if a new device has been discovered
while (num < numaddrs) {
if (strcmp (addr, addrs[num]) == 0) {
matchfound = 1;
}
num++;
}
// Run commands for newly found device address
if (matchfound == 0) {
time_stamp(addr, name, log_file);
run_config(addr, log_file, filename);
// Add address to list of discovered devices
addrs[numaddrs] = malloc(sizeof addr + 1);
strcpy(addrs[numaddrs], addr);
numaddrs++;
}
}
free( ii );
close( sock );
return 0;
}
static void read_info_complete(int sk, uint16_t index, void *buf, size_t len)
{
struct mgmt_rp_read_info *rp = buf;
struct controller_info *info;
struct btd_adapter *adapter;
uint8_t mode;
char addr[18];
if (len < sizeof(*rp)) {
error("Too small read info complete event");
return;
}
if (index > max_index) {
error("Unexpected index %u in read info complete", index);
return;
}
mgmt_set_mode(index, MGMT_OP_SET_SERVICE_CACHE, 1);
info = &controllers[index];
info->type = rp->type;
info->enabled = rp->powered;
info->connectable = rp->connectable;
info->discoverable = rp->discoverable;
info->pairable = rp->pairable;
info->sec_mode = rp->sec_mode;
bacpy(&info->bdaddr, &rp->bdaddr);
memcpy(info->dev_class, rp->dev_class, 3);
memcpy(info->features, rp->features, 8);
info->manufacturer = btohs(bt_get_unaligned(&rp->manufacturer));
info->hci_ver = rp->hci_ver;
info->hci_rev = btohs(bt_get_unaligned(&rp->hci_rev));
ba2str(&info->bdaddr, addr);
DBG("hci%u type %u addr %s", index, info->type, addr);
DBG("hci%u class 0x%02x%02x%02x", index,
info->dev_class[2], info->dev_class[1], info->dev_class[0]);
DBG("hci%u manufacturer %d HCI ver %d:%d", index, info->manufacturer,
info->hci_ver, info->hci_rev);
DBG("hci%u enabled %u discoverable %u pairable %u sec_mode %u", index,
info->enabled, info->discoverable,
info->pairable, info->sec_mode);
DBG("hci%u name %s", index, (char *) rp->name);
adapter = btd_manager_register_adapter(index);
if (adapter == NULL) {
error("mgmtops: unable to register adapter");
return;
}
btd_adapter_get_mode(adapter, &mode, NULL, NULL);
if (mode == MODE_OFF) {
mgmt_set_powered(index, FALSE);
return;
}
if (info->enabled)
mgmt_update_powered(index, TRUE);
else
mgmt_set_powered(index, TRUE);
adapter_update_local_name(adapter, (char *) rp->name);
btd_adapter_unref(adapter);
}
static int hidp_add_connection(struct input_device *idev)
{
struct hidp_connadd_req *req;
sdp_record_t *rec;
char src_addr[18], dst_addr[18];
char filename[PATH_MAX + 1];
GKeyFile *key_file;
char handle[11], *str;
GError *gerr = NULL;
int err;
req = g_new0(struct hidp_connadd_req, 1);
req->ctrl_sock = g_io_channel_unix_get_fd(idev->ctrl_io);
req->intr_sock = g_io_channel_unix_get_fd(idev->intr_io);
req->flags = 0;
req->idle_to = idle_timeout;
ba2str(&idev->src, src_addr);
ba2str(&idev->dst, dst_addr);
snprintf(filename, PATH_MAX, STORAGEDIR "/%s/cache/%s", src_addr,
dst_addr);
filename[PATH_MAX] = '\0';
sprintf(handle, "0x%8.8X", idev->handle);
key_file = g_key_file_new();
g_key_file_load_from_file(key_file, filename, 0, NULL);
str = g_key_file_get_string(key_file, "ServiceRecords", handle, NULL);
g_key_file_free(key_file);
if (!str) {
error("Rejected connection from unknown device %s", dst_addr);
err = -EPERM;
goto cleanup;
}
rec = record_from_string(str);
g_free(str);
err = extract_hid_record(rec, req);
sdp_record_free(rec);
if (err < 0) {
error("Could not parse HID SDP record: %s (%d)", strerror(-err),
-err);
goto cleanup;
}
req->vendor = btd_device_get_vendor(idev->device);
req->product = btd_device_get_product(idev->device);
req->version = btd_device_get_version(idev->device);
if (idev->name)
strncpy(req->name, idev->name, sizeof(req->name) - 1);
/* Encryption is mandatory for keyboards */
if (req->subclass & 0x40) {
if (!bt_io_set(idev->intr_io, &gerr,
BT_IO_OPT_SEC_LEVEL, BT_IO_SEC_MEDIUM,
BT_IO_OPT_INVALID)) {
error("btio: %s", gerr->message);
g_error_free(gerr);
err = -EFAULT;
goto cleanup;
}
idev->req = req;
idev->sec_watch = g_io_add_watch(idev->intr_io, G_IO_OUT,
encrypt_notify, idev);
return 0;
}
err = ioctl_connadd(req);
cleanup:
g_free(req->rd_data);
g_free(req);
return err;
}
void peisk_bluetoothScan() {
int i,j;
unsigned char scanRequest[5] = {0x33,0x8b,0x9e,PEISK_BT_SCAN_LENGTH,0x00};
static unsigned char *buf = NULL , *ctrl = NULL;
int len, hdr_size = HCIDUMP_HDR_SIZE;
struct cmsghdr *cmsg;
struct msghdr msg;
struct iovec iv;
struct hcidump_hdr *dh;
struct btsnoop_pkt *dp;
struct frame frm;
char str[256], val[256];
int n;
/* Go through all known bluetooth interfaces and
initiate scans on those that are not currenly scanning and have
passed their time.
*/
for(n=0;n<peisk_nBluetoothAdaptors;n++) {
PeisBluetoothAdaptor *adaptor = &peisk_bluetoothAdaptors[n];
if(adaptor->interfaceSocket < 0) continue;
//printf("Initiating scan on device %d\n",n);
/* Handle special case if time goes backwards. */
if(adaptor->nextScan > peisk_timeNow+PEISK_BT_SCAN_PERIOD)
adaptor->nextScan = peisk_timeNow+PEISK_BT_SCAN_PERIOD;
/* Initiate new scans */
if(adaptor->nextScan < peisk_timeNow) {
adaptor->nextScan = peisk_timeNow + PEISK_BT_SCAN_PERIOD;
adaptor->isScanning=1;
/*printf("Initiating scan on %s\n",adaptor->name);*/
fflush(stdout);
hci_send_cmd(adaptor->interfaceSocket,0x01,0x01,sizeof(scanRequest),scanRequest);
}
/* Process any frames on the raw sockets */
int sock = adaptor->interfaceSocket;
int snap_len = HCI_MAX_FRAME_SIZE;
if(!buf)
buf = malloc(snap_len + hdr_size);
if(!buf) {
perror("Can't allocate data buffer for bluetooth devices");
return;
}
dh = (void *) buf;
dp = (void *) buf;
frm.data = buf + hdr_size;
if(!ctrl)
ctrl = malloc(100);
if (!ctrl) {
free(buf);
perror("Can't allocate control buffer");
return;
}
memset(&msg, 0, sizeof(msg));
iv.iov_base = frm.data;
iv.iov_len = snap_len;
msg.msg_iov = &iv;
msg.msg_iovlen = 1;
msg.msg_control = ctrl;
msg.msg_controllen = 100;
len = recvmsg(sock, &msg, MSG_DONTWAIT);
if (len < 0) {
if (errno == EAGAIN || errno == EINTR)
return;
return;
}
printf("Read %d bytes from bluetooth socket %d\n",len,sock);
/* Process control mes printf("Read data is\n"sage */
frm.data_len = len;
frm.in = 0;
cmsg = CMSG_FIRSTHDR(&msg);
while (cmsg) {
switch (cmsg->cmsg_type) {
case HCI_CMSG_DIR:
frm.in = *((int *) CMSG_DATA(cmsg));
break;
case HCI_CMSG_TSTAMP:
frm.ts = *((struct timeval *) CMSG_DATA(cmsg));
break;
}
cmsg = CMSG_NXTHDR(&msg, cmsg);
}
frm.ptr = frm.data;
frm.len = frm.data_len;
unsigned char btAddr[6];
unsigned char *data = frm.data;
printf("frm.len = %d\n",frm.len);
peisk_hexDump(data,len);
if(frm.len == 4 &&
data[0] == 0x04 &&
data[1] == 0x01 &&
data[2] == 0x01 &&
data[3] == 0x00) {
adaptor->isScanning = 0;
/*printf("Interface %s is FINISHED\n",adaptor->name);*/
}
/* Short "normal" inquiry results */
if(frm.len == 18 &&
data[0] == 0x04 &&
data[1] == 0x22 &&
data[2] == 0x0f) {
for(i=0;i<6;i++) btAddr[i] = data[9-i];
for(i=0;i<peisk_nBluetoothDevices;i++) {
for(j=0;j<6;j++)
if(btAddr[j] != peisk_bluetoothDevices[i].btAddr[j]) break;
if(j == 6) break;
/*else printf("does not match %d (j=%d)\n",i,j);*/
}
if(i == peisk_nBluetoothDevices) {
/* No previous device found */
peisk_nBluetoothDevices++;
for(j=0;j<6;j++) peisk_bluetoothDevices[i].btAddr[j] = btAddr[j];
for(j=0;j<PEISK_MAX_BLUETOOTH_ADAPTORS;j++) {
peisk_bluetoothDevices[i].rawStrength[j]=0;
peisk_bluetoothDevices[i].lastSeen[j]=0.0;
}
peisk_bluetoothDevices[i].name[0]=0;
peisk_bluetoothDevices[i].nextHelloAttempt=0.0;
peisk_bluetoothDevices[i].isConnectable=0;
printf("Found a new device: ");
for(j=0;j<6;j++) printf("%02x:",peisk_bluetoothDevices[i].btAddr[j]);
printf("\n");
}
if(peisk_bluetoothDevices[i].name[0] == 0 && 0) {
/** \todo Read names of bluetooth devices asynchronosly */
/* Scan for name of this device */
bdaddr_t baddr;
for(j=0;j<6;j++) baddr.b[j] = btAddr[5-j];
printf("Attempting to read remote name of: ");
ba2str(&baddr,val);
printf("%s\n",val);
int s = hci_open_dev(adaptor->id);
if(hci_read_remote_name(s,&baddr,255,val,500) == 0) {
printf("Read name: %s\n",val);
strncpy(peisk_bluetoothDevices[i].name,val,255);
sprintf(str,"kernel.%s.%02X%02X%02X%02X%02X%02X.name",adaptor->name,
btAddr[0],btAddr[1],btAddr[2],btAddr[3],btAddr[4],btAddr[5]);
peisk_setStringTuple(str,val);
} else
perror("Failed");
close(s);
}
peisk_bluetoothDevices[i].lastSeen[n] = peisk_timeNow;
peisk_bluetoothDevices[i].rawStrength[n] = data[17];
/*printf("Device %d, iface %s <- %d\n",i,adaptor->name,data[17]);*/
sprintf(str,"kernel.%s.%02X%02X%02X%02X%02X%02X.str",adaptor->name,
btAddr[0],btAddr[1],btAddr[2],btAddr[3],btAddr[4],btAddr[5]);
sprintf(val,"%d",data[17]);
peisk_setStringTuple(str,val);
}
else if(frm.len == 258 &&
data[0] == 0x04 &&
data[1] == 0x2f &&
data[2] == 0xff &&
data[3] == 0x01) {
for(i=0;i<6;i++) btAddr[i] = data[9-i];
for(i=0;i<peisk_nBluetoothDevices;i++) {
for(j=0;j<6;j++)
if(btAddr[j] != peisk_bluetoothDevices[i].btAddr[j]) break;
if(j == 6) break;
/*else printf("does not match %d (j=%d)\n",i,j);*/
}
if(i == peisk_nBluetoothDevices) {
/* No previous device found */
peisk_nBluetoothDevices++;
for(j=0;j<6;j++) peisk_bluetoothDevices[i].btAddr[j] = btAddr[j];
for(j=0;j<PEISK_MAX_BLUETOOTH_ADAPTORS;j++) {
peisk_bluetoothDevices[i].rawStrength[j]=0;
peisk_bluetoothDevices[i].lastSeen[j]=0.0;
}
peisk_bluetoothDevices[i].name[0]=0;
peisk_bluetoothDevices[i].nextHelloAttempt=0.0;
peisk_bluetoothDevices[i].isConnectable=0;
printf("Found a new device: ");
for(j=0;j<6;j++) printf("%02x:",peisk_bluetoothDevices[i].btAddr[j]);
printf("\n");
}
peisk_bluetoothDevices[i].lastSeen[n] = peisk_timeNow;
peisk_bluetoothDevices[i].rawStrength[n] = data[17];
sprintf(str,"kernel.%s.%02X%02X%02X%02X%02X%02X.str",adaptor->name,
btAddr[0],btAddr[1],btAddr[2],btAddr[3],btAddr[4],btAddr[5]);
sprintf(val,"%d",data[17]);
peisk_setStringTuple(str,val);
}
}
}