Ejemplo n.º 1
0
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;
  }
}
Ejemplo n.º 2
0
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);
	}
Ejemplo n.º 5
0
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);
	}
}
Ejemplo n.º 6
0
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;
}
Ejemplo n.º 7
0
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;
}
Ejemplo n.º 8
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);
}
Ejemplo n.º 11
0
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");
		*/
}
Ejemplo n.º 13
0
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;
}
Ejemplo n.º 14
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);
}
Ejemplo n.º 16
0
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;
}
Ejemplo n.º 17
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;
	}
}
Ejemplo n.º 18
0
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;
}
Ejemplo n.º 19
0
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;
}
Ejemplo n.º 20
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;
}
Ejemplo n.º 21
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;
}
Ejemplo n.º 22
0
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);
        }
      }
    }
  }
}
Ejemplo n.º 23
0
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]);
  }
}
Ejemplo n.º 24
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;
}
Ejemplo n.º 25
0
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);
}
Ejemplo n.º 26
0
//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;
}
Ejemplo n.º 27
0
// 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;

}
Ejemplo n.º 28
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);
}
Ejemplo n.º 29
0
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;
}
Ejemplo n.º 30
0
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);
    }
  }
}