lib/libbluetooth/hci.c

   1 /*
   2  * hci.c
   3  */
   4
   5 /*-
   6  * SPDX-License-Identifier: BSD-2-Clause-FreeBSD
   7  *
   8  * Copyright (c) 2009 Maksim Yevmenkin <m_evmenkin@yahoo.com>
   9  * All rights reserved.
  10  *
  11  * Redistribution and use in source and binary forms, with or without
  12  * modification, are permitted provided that the following conditions
  13  * are met:
  14  * 1. Redistributions of source code must retain the above copyright
  15  *    notice, this list of conditions and the following disclaimer.
  16  * 2. Redistributions in binary form must reproduce the above copyright
  17  *    notice, this list of conditions and the following disclaimer in the
  18  *    documentation and/or other materials provided with the distribution.
  19  *
  20  * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
  21  * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
  22  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
  23  * ARE DISCLAIMED. IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
  24  * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
  25  * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
  26  * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
  27  * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
  28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
  29  * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
  30  * SUCH DAMAGE.
  31  *
  32  * $FreeBSD$
  33  */
  34
  35 #include <assert.h>
  36 #define L2CAP_SOCKET_CHECKED
  37 #include <bluetooth.h>
  38 #include <inttypes.h>
  39 #include <stdio.h>
  40 #include <stdlib.h>
  41 #include <string.h>
  42 #include <unistd.h>
  43
  44 #undef  MIN
  45 #define MIN(a, b)       (((a) < (b))? (a) : (b))
  46
  47 static int    bt_devany_cb(int s, struct bt_devinfo const *di, void *xdevname);
  48 static char * bt_dev2node (char const *devname, char *nodename, int nnlen);
  49
  50 int
  51 bt_devopen(char const *devname)
  52 {
  53         struct sockaddr_hci     ha;
  54         bdaddr_t                ba;
  55         int                     s;
  56
  57         if (devname == NULL) {
  58                 errno = EINVAL;
  59                 return (-1);
  60         }
  61
  62         memset(&ha, 0, sizeof(ha));
  63         ha.hci_len = sizeof(ha);
  64         ha.hci_family = AF_BLUETOOTH;
  65
  66         if (bt_aton(devname, &ba)) {
  67                 if (!bt_devname(ha.hci_node, &ba))
  68                         return (-1);
  69         } else if (bt_dev2node(devname, ha.hci_node,
  70                                         sizeof(ha.hci_node)) == NULL) {
  71                 errno = ENXIO;
  72                 return (-1);
  73         }
  74
  75         s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_HCI);
  76         if (s < 0)
  77                 return (-1);
  78
  79         if (bind(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
  80             connect(s, (struct sockaddr *) &ha, sizeof(ha)) < 0) {
  81                 close(s);
  82                 return (-1);
  83         }
  84
  85         return (s);
  86 }
  87
  88 int
  89 bt_devclose(int s)
  90 {
  91         return (close(s));
  92 }
  93
  94 int
  95 bt_devsend(int s, uint16_t opcode, void *param, size_t plen)
  96 {
  97         ng_hci_cmd_pkt_t        h;
  98         struct iovec            iv[2];
  99         int                     ivn;
 100
 101         if ((plen == 0 && param != NULL) ||
 102             (plen > 0 && param == NULL) ||
 103             plen > UINT8_MAX) {
 104                 errno = EINVAL;
 105                 return (-1);
 106         }
 107
 108         iv[0].iov_base = &h;
 109         iv[0].iov_len = sizeof(h);
 110         ivn = 1;
 111
 112         h.type = NG_HCI_CMD_PKT;
 113         h.opcode = htole16(opcode);
 114         if (plen > 0) {
 115                 h.length = plen;
 116
 117                 iv[1].iov_base = param;
 118                 iv[1].iov_len = plen;
 119                 ivn = 2;
 120         } else
 121                 h.length = 0;
 122
 123         while (writev(s, iv, ivn) < 0) {
 124                 if (errno == EAGAIN || errno == EINTR)
 125                         continue;
 126
 127                 return (-1);
 128         }
 129
 130         return (0);
 131 }
 132
 133 ssize_t
 134 bt_devrecv(int s, void *buf, size_t size, time_t to)
 135 {
 136         ssize_t n;
 137
 138         if (buf == NULL || size == 0) {
 139                 errno = EINVAL;
 140                 return (-1);
 141         }
 142
 143         if (to >= 0) {
 144                 fd_set          rfd;
 145                 struct timeval  tv;
 146
 147                 FD_ZERO(&rfd);
 148                 FD_SET(s, &rfd);
 149
 150                 tv.tv_sec = to;
 151                 tv.tv_usec = 0;
 152
 153                 while ((n = select(s + 1, &rfd, NULL, NULL, &tv)) < 0) {
 154                         if (errno == EAGAIN || errno == EINTR)
 155                                 continue;
 156
 157                         return (-1);
 158                 }
 159
 160                 if (n == 0) {
 161                         errno = ETIMEDOUT;
 162                         return (-1);
 163                 }
 164
 165                 assert(FD_ISSET(s, &rfd));
 166         }
 167
 168         while ((n = read(s, buf, size)) < 0) {
 169                 if (errno == EAGAIN || errno == EINTR)
 170                         continue;
 171
 172                 return (-1);
 173         }
 174
 175         switch (*((uint8_t *) buf)) {
 176         case NG_HCI_CMD_PKT: {
 177                 ng_hci_cmd_pkt_t        *h = (ng_hci_cmd_pkt_t *) buf;
 178
 179                 if (n >= sizeof(*h) && n == (sizeof(*h) + h->length))
 180                         return (n);
 181                 } break;
 182
 183         case NG_HCI_ACL_DATA_PKT: {
 184                 ng_hci_acldata_pkt_t    *h = (ng_hci_acldata_pkt_t *) buf;
 185
 186                 if (n >= sizeof(*h) && n == (sizeof(*h) + le16toh(h->length)))
 187                         return (n);
 188                 } break;
 189
 190         case NG_HCI_SCO_DATA_PKT: {
 191                 ng_hci_scodata_pkt_t    *h = (ng_hci_scodata_pkt_t *) buf;
 192
 193                 if (n >= sizeof(*h) && n == (sizeof(*h) + h->length))
 194                         return (n);
 195                 } break;
 196
 197         case NG_HCI_EVENT_PKT: {
 198                 ng_hci_event_pkt_t      *h = (ng_hci_event_pkt_t *) buf;
 199
 200                 if (n >= sizeof(*h) && n == (sizeof(*h) + h->length))
 201                         return (n);
 202                 } break;
 203         }
 204
 205         errno = EIO;
 206         return (-1);
 207 }
 208
 209 int
 210 bt_devreq(int s, struct bt_devreq *r, time_t to)
 211 {
 212         uint8_t                         buf[320]; /* more than enough */
 213         ng_hci_event_pkt_t              *e = (ng_hci_event_pkt_t *) buf;
 214         ng_hci_command_compl_ep         *cc = (ng_hci_command_compl_ep *)(e+1);
 215         ng_hci_command_status_ep        *cs = (ng_hci_command_status_ep*)(e+1);
 216         struct bt_devfilter             old, new;
 217         time_t                          t_end;
 218         uint16_t                        opcode;
 219         ssize_t                         n;
 220         int                             error;
 221
 222         if (s < 0 || r == NULL || to < 0) {
 223                 errno = EINVAL;
 224                 return (-1);
 225         }
 226
 227         if ((r->rlen == 0 && r->rparam != NULL) ||
 228             (r->rlen > 0 && r->rparam == NULL)) {
 229                 errno = EINVAL;
 230                 return (-1);
 231         }
 232
 233         memset(&new, 0, sizeof(new));
 234         bt_devfilter_pkt_set(&new, NG_HCI_EVENT_PKT);
 235         bt_devfilter_evt_set(&new, NG_HCI_EVENT_COMMAND_COMPL);
 236         bt_devfilter_evt_set(&new, NG_HCI_EVENT_COMMAND_STATUS);
 237         if (r->event != 0)
 238                 bt_devfilter_evt_set(&new, r->event);
 239
 240         if (bt_devfilter(s, &new, &old) < 0)
 241                 return (-1);
 242
 243         error = 0;
 244
 245         n = bt_devsend(s, r->opcode, r->cparam, r->clen);
 246         if (n < 0) {
 247                 error = errno;
 248                 goto out;
 249         }
 250
 251         opcode = htole16(r->opcode);
 252         t_end = time(NULL) + to;
 253
 254         do {
 255                 to = t_end - time(NULL);
 256                 if (to < 0)
 257                         to = 0;
 258
 259                 n = bt_devrecv(s, buf, sizeof(buf), to);
 260                 if (n < 0) {
 261                         error = errno;
 262                         goto out;
 263                 }
 264
 265                 if (e->type != NG_HCI_EVENT_PKT) {
 266                         error = EIO;
 267                         goto out;
 268                 }
 269
 270                 n -= sizeof(*e);
 271
 272                 switch (e->event) {
 273                 case NG_HCI_EVENT_COMMAND_COMPL:
 274                         if (cc->opcode == opcode) {
 275                                 n -= sizeof(*cc);
 276
 277                                 if (r->rlen >= n) {
 278                                         r->rlen = n;
 279                                         memcpy(r->rparam, cc + 1, r->rlen);
 280                                 }
 281
 282                                 goto out;
 283                         }
 284                         break;
 285
 286                 case NG_HCI_EVENT_COMMAND_STATUS:
 287                         if (cs->opcode == opcode) {
 288                                 if (r->event != NG_HCI_EVENT_COMMAND_STATUS) {
 289                                         if (cs->status != 0) {
 290                                                 error = EIO;
 291                                                 goto out;
 292                                         }
 293                                 } else {
 294                                         if (r->rlen >= n) {
 295                                                 r->rlen = n;
 296                                                 memcpy(r->rparam, cs, r->rlen);
 297                                         }
 298
 299                                         goto out;
 300                                 }
 301                         }
 302                         break;
 303
 304                 default:
 305                         if (e->event == r->event) {
 306                                 if (r->rlen >= n) {
 307                                         r->rlen = n;
 308                                         memcpy(r->rparam, e + 1, r->rlen);
 309                                 }
 310
 311                                 goto out;
 312                         }
 313                         break;
 314                 }
 315         } while (to > 0);
 316
 317         error = ETIMEDOUT;
 318 out:
 319         bt_devfilter(s, &old, NULL);
 320
 321         if (error != 0) {
 322                 errno = error;
 323                 return (-1);
 324         }
 325
 326         return (0);
 327 }
 328
 329 int
 330 bt_devfilter(int s, struct bt_devfilter const *new, struct bt_devfilter *old)
 331 {
 332         struct ng_btsocket_hci_raw_filter       f;
 333         socklen_t                               len;
 334
 335         if (new == NULL && old == NULL) {
 336                 errno = EINVAL;
 337                 return (-1);
 338         }
 339
 340         if (old != NULL) {
 341                 len = sizeof(f);
 342                 if (getsockopt(s, SOL_HCI_RAW, SO_HCI_RAW_FILTER, &f, &len) < 0)
 343                         return (-1);
 344
 345                 memset(old, 0, sizeof(*old));
 346                 memcpy(old->packet_mask, &f.packet_mask,
 347                         MIN(sizeof(old->packet_mask), sizeof(f.packet_mask)));
 348                 memcpy(old->event_mask, &f.event_mask,
 349                         MIN(sizeof(old->event_mask), sizeof(f.packet_mask)));
 350         }
 351
 352         if (new != NULL) {
 353                 memset(&f, 0, sizeof(f));
 354                 memcpy(&f.packet_mask, new->packet_mask,
 355                         MIN(sizeof(f.packet_mask), sizeof(new->event_mask)));
 356                 memcpy(&f.event_mask, new->event_mask,
 357                         MIN(sizeof(f.event_mask), sizeof(new->event_mask)));
 358
 359                 len = sizeof(f);
 360                 if (setsockopt(s, SOL_HCI_RAW, SO_HCI_RAW_FILTER, &f, len) < 0)
 361                         return (-1);
 362         }
 363
 364         return (0);
 365 }
 366
 367 void
 368 bt_devfilter_pkt_set(struct bt_devfilter *filter, uint8_t type)
 369 {
 370         bit_set(filter->packet_mask, type - 1);
 371 }
 372
 373 void
 374 bt_devfilter_pkt_clr(struct bt_devfilter *filter, uint8_t type)
 375 {
 376         bit_clear(filter->packet_mask, type - 1);
 377 }
 378
 379 int
 380 bt_devfilter_pkt_tst(struct bt_devfilter const *filter, uint8_t type)
 381 {
 382         return (bit_test(filter->packet_mask, type - 1));
 383 }
 384
 385 void
 386 bt_devfilter_evt_set(struct bt_devfilter *filter, uint8_t event)
 387 {
 388         bit_set(filter->event_mask, event - 1);
 389 }
 390
 391 void
 392 bt_devfilter_evt_clr(struct bt_devfilter *filter, uint8_t event)
 393 {
 394         bit_clear(filter->event_mask, event - 1);
 395 }
 396
 397 int
 398 bt_devfilter_evt_tst(struct bt_devfilter const *filter, uint8_t event)
 399 {
 400         return (bit_test(filter->event_mask, event - 1));
 401 }
 402
 403 int
 404 bt_devinquiry(char const *devname, time_t length, int num_rsp,
 405                 struct bt_devinquiry **ii)
 406 {
 407         uint8_t                         buf[320];
 408         char                            _devname[HCI_DEVNAME_SIZE];
 409         struct bt_devfilter             f;
 410         ng_hci_inquiry_cp               *cp = (ng_hci_inquiry_cp *) buf;
 411         ng_hci_event_pkt_t              *e = (ng_hci_event_pkt_t *) buf;
 412         ng_hci_inquiry_result_ep        *ep = (ng_hci_inquiry_result_ep *)(e+1);
 413         ng_hci_inquiry_response         *ir;
 414         struct bt_devinquiry            *i;
 415         int                             s, n;
 416
 417         if (ii == NULL) {
 418                 errno = EINVAL;
 419                 return (-1);
 420         }
 421
 422         if (devname == NULL) {
 423                 memset(_devname, 0, sizeof(_devname));
 424                 devname = _devname;
 425
 426                 n = bt_devenum(bt_devany_cb, _devname);
 427                 if (n <= 0) {
 428                         if (n == 0)
 429                                 *ii = NULL;
 430
 431                         return (n);
 432                 }
 433         }
 434
 435         s = bt_devopen(devname);
 436         if (s < 0)
 437                 return (-1);
 438
 439         if (bt_devfilter(s, NULL, &f) < 0) {
 440                 bt_devclose(s);
 441                 return (-1);
 442         }
 443
 444         bt_devfilter_evt_set(&f, NG_HCI_EVENT_INQUIRY_COMPL);
 445         bt_devfilter_evt_set(&f, NG_HCI_EVENT_INQUIRY_RESULT);
 446
 447         if (bt_devfilter(s, &f, NULL) < 0) {
 448                 bt_devclose(s);
 449                 return (-1);
 450         }
 451
 452         /* Always use GIAC LAP */
 453         cp->lap[0] = 0x33;
 454         cp->lap[1] = 0x8b;
 455         cp->lap[2] = 0x9e;
 456
 457         /*
 458          * Calculate inquire length in 1.28 second units
 459          * v2.x specification says that 1.28 -> 61.44 seconds
 460          * range is acceptable
 461          */
 462
 463         if (length <= 0)
 464                 length = 5;
 465         else if (length == 1)
 466                 length = 2;
 467         else if (length > 62)
 468                 length = 62;
 469
 470         cp->inquiry_length = (uint8_t)((length * 100) / 128);
 471
 472         if (num_rsp <= 0 || num_rsp > 255)
 473                 num_rsp = 8;
 474         cp->num_responses = (uint8_t) num_rsp;
 475
 476         i = *ii = calloc(num_rsp, sizeof(struct bt_devinquiry));
 477         if (i == NULL) {
 478                 bt_devclose(s);
 479                 errno = ENOMEM;
 480                 return (-1);
 481         }
 482
 483         if (bt_devsend(s,
 484                 NG_HCI_OPCODE(NG_HCI_OGF_LINK_CONTROL, NG_HCI_OCF_INQUIRY),
 485                         cp, sizeof(*cp)) < 0) {
 486                 free(i);
 487                 bt_devclose(s);
 488                 return (-1);
 489         }
 490
 491 wait_for_more:
 492
 493         n = bt_devrecv(s, buf, sizeof(buf), length);
 494         if (n < 0) {
 495                 free(i);
 496                 bt_devclose(s);
 497                 return (-1);
 498         }
 499
 500         if (n < sizeof(ng_hci_event_pkt_t)) {
 501                 free(i);
 502                 bt_devclose(s);
 503                 errno = EIO;
 504                 return (-1);
 505         }
 506
 507         switch (e->event) {
 508         case NG_HCI_EVENT_INQUIRY_COMPL:
 509                 break;
 510
 511         case NG_HCI_EVENT_INQUIRY_RESULT:
 512                 ir = (ng_hci_inquiry_response *)(ep + 1);
 513
 514                 for (n = 0; n < MIN(ep->num_responses, num_rsp); n ++) {
 515                         bdaddr_copy(&i->bdaddr, &ir->bdaddr);
 516                         i->pscan_rep_mode = ir->page_scan_rep_mode;
 517                         i->pscan_period_mode = ir->page_scan_period_mode;
 518                         memcpy(i->dev_class, ir->uclass, sizeof(i->dev_class));
 519                         i->clock_offset = le16toh(ir->clock_offset);
 520
 521                         ir ++;
 522                         i ++;
 523                         num_rsp --;
 524                 }
 525                 /* FALLTHROUGH */
 526
 527         default:
 528                 goto wait_for_more;
 529                 /* NOT REACHED */
 530         }
 531
 532         bt_devclose(s);
 533
 534         return (i - *ii);
 535 }
 536
 537 int
 538 bt_devinfo(struct bt_devinfo *di)
 539 {
 540         union {
 541                 struct ng_btsocket_hci_raw_node_state           r0;
 542                 struct ng_btsocket_hci_raw_node_bdaddr          r1;
 543                 struct ng_btsocket_hci_raw_node_features        r2;
 544                 struct ng_btsocket_hci_raw_node_buffer          r3;
 545                 struct ng_btsocket_hci_raw_node_stat            r4;
 546                 struct ng_btsocket_hci_raw_node_link_policy_mask r5;
 547                 struct ng_btsocket_hci_raw_node_packet_mask     r6;
 548                 struct ng_btsocket_hci_raw_node_role_switch     r7;
 549                 struct ng_btsocket_hci_raw_node_debug           r8;
 550         }                                               rp;
 551         struct sockaddr_hci                             ha;
 552         socklen_t                                       halen;
 553         int                                             s, rval;
 554
 555         if (di == NULL) {
 556                 errno = EINVAL;
 557                 return (-1);
 558         }
 559
 560         s = bt_devopen(di->devname);
 561         if (s < 0)
 562                 return (-1);
 563
 564         rval = -1;
 565
 566         halen = sizeof(ha);
 567         if (getsockname(s, (struct sockaddr *) &ha, &halen) < 0)
 568                 goto bad;
 569         strlcpy(di->devname, ha.hci_node, sizeof(di->devname));
 570
 571         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_STATE, &rp.r0, sizeof(rp.r0)) < 0)
 572                 goto bad;
 573         di->state = rp.r0.state;
 574
 575         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BDADDR, &rp.r1, sizeof(rp.r1)) < 0)
 576                 goto bad;
 577         bdaddr_copy(&di->bdaddr, &rp.r1.bdaddr);
 578
 579         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_FEATURES, &rp.r2, sizeof(rp.r2)) < 0)
 580                 goto bad;
 581         memcpy(di->features, rp.r2.features, sizeof(di->features));
 582
 583         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_BUFFER, &rp.r3, sizeof(rp.r3)) < 0)
 584                 goto bad;
 585         di->cmd_free = rp.r3.buffer.cmd_free;
 586         di->sco_size = rp.r3.buffer.sco_size;
 587         di->sco_pkts = rp.r3.buffer.sco_pkts;
 588         di->sco_free = rp.r3.buffer.sco_free;
 589         di->acl_size = rp.r3.buffer.acl_size;
 590         di->acl_pkts = rp.r3.buffer.acl_pkts;
 591         di->acl_free = rp.r3.buffer.acl_free;
 592
 593         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_STAT, &rp.r4, sizeof(rp.r4)) < 0)
 594                 goto bad;
 595         di->cmd_sent = rp.r4.stat.cmd_sent;
 596         di->evnt_recv = rp.r4.stat.evnt_recv;
 597         di->acl_recv = rp.r4.stat.acl_recv;
 598         di->acl_sent = rp.r4.stat.acl_sent;
 599         di->sco_recv = rp.r4.stat.sco_recv;
 600         di->sco_sent = rp.r4.stat.sco_sent;
 601         di->bytes_recv = rp.r4.stat.bytes_recv;
 602         di->bytes_sent = rp.r4.stat.bytes_sent;
 603
 604         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_LINK_POLICY_MASK,
 605                         &rp.r5, sizeof(rp.r5)) < 0)
 606                 goto bad;
 607         di->link_policy_info = rp.r5.policy_mask;
 608
 609         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_PACKET_MASK,
 610                         &rp.r6, sizeof(rp.r6)) < 0)
 611                 goto bad;
 612         di->packet_type_info = rp.r6.packet_mask;
 613
 614          if (ioctl(s, SIOC_HCI_RAW_NODE_GET_ROLE_SWITCH,
 615                         &rp.r7, sizeof(rp.r7)) < 0)
 616                 goto bad;
 617         di->role_switch_info = rp.r7.role_switch;
 618
 619         if (ioctl(s, SIOC_HCI_RAW_NODE_GET_DEBUG, &rp.r8, sizeof(rp.r8)) < 0)
 620                 goto bad;
 621         di->debug = rp.r8.debug;
 622
 623         rval = 0;
 624 bad:
 625         bt_devclose(s);
 626
 627         return (rval);
 628 }
 629
 630 int
 631 bt_devenum(bt_devenum_cb_t cb, void *arg)
 632 {
 633         struct ng_btsocket_hci_raw_node_list_names      rp;
 634         struct bt_devinfo                               di;
 635         struct sockaddr_hci                             ha;
 636         int                                             s, i, count;
 637
 638         rp.num_names = HCI_DEVMAX;
 639         rp.names = (struct nodeinfo *) calloc(rp.num_names,
 640                                                 sizeof(struct nodeinfo));
 641         if (rp.names == NULL) {
 642                 errno = ENOMEM;
 643                 return (-1);
 644         }
 645
 646         memset(&ha, 0, sizeof(ha));
 647         ha.hci_len = sizeof(ha);
 648         ha.hci_family = AF_BLUETOOTH;
 649         ha.hci_node[0] = 'x';
 650
 651         s = socket(PF_BLUETOOTH, SOCK_RAW, BLUETOOTH_PROTO_HCI);
 652         if (s < 0) {
 653                 free(rp.names);
 654
 655                 return (-1);
 656         }
 657
 658         if (bind(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
 659             connect(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
 660             ioctl(s, SIOC_HCI_RAW_NODE_LIST_NAMES, &rp, sizeof(rp)) < 0) {
 661                 close(s);
 662                 free(rp.names);
 663
 664                 return (-1);
 665         }
 666
 667         for (count = 0, i = 0; i < rp.num_names; i ++) {
 668                 strlcpy(di.devname, rp.names[i].name, sizeof(di.devname));
 669                 if (bt_devinfo(&di) < 0)
 670                         continue;
 671
 672                 count ++;
 673
 674                 if (cb == NULL)
 675                         continue;
 676
 677                 strlcpy(ha.hci_node, rp.names[i].name, sizeof(ha.hci_node));
 678                 if (bind(s, (struct sockaddr *) &ha, sizeof(ha)) < 0 ||
 679                     connect(s, (struct sockaddr *) &ha, sizeof(ha)) < 0)
 680                         continue;
 681
 682                 if ((*cb)(s, &di, arg) > 0)
 683                         break;
 684         }
 685
 686         close (s);
 687         free(rp.names);
 688
 689         return (count);
 690 }
 691
 692 static int
 693 bt_devany_cb(int s, struct bt_devinfo const *di, void *xdevname)
 694 {
 695         strlcpy((char *) xdevname, di->devname, HCI_DEVNAME_SIZE);
 696         return (1);
 697 }
 698
 699 static char *
 700 bt_dev2node(char const *devname, char *nodename, int nnlen)
 701 {
 702         static char const *      bt_dev_prefix[] = {
 703                 "btccc",        /* 3Com Bluetooth PC-CARD */
 704                 "h4",           /* UART/serial Bluetooth devices */
 705                 "ubt",          /* Bluetooth USB devices */
 706                 NULL            /* should be last */
 707         };
 708
 709         static char             _nodename[HCI_DEVNAME_SIZE];
 710         char const              **p;
 711         char                    *ep;
 712         int                     plen, unit;
 713
 714         if (nodename == NULL) {
 715                 nodename = _nodename;
 716                 nnlen = HCI_DEVNAME_SIZE;
 717         }
 718
 719         for (p = bt_dev_prefix; *p != NULL; p ++) {
 720                 plen = strlen(*p);
 721                 if (strncmp(devname, *p, plen) != 0)
 722                         continue;
 723
 724                 unit = strtoul(devname + plen, &ep, 10);
 725                 if (*ep != '\0' &&
 726                     strcmp(ep, "hci") != 0 &&
 727                     strcmp(ep, "l2cap") != 0)
 728                         return (NULL);  /* can't make sense of device name */
 729
 730                 snprintf(nodename, nnlen, "%s%uhci", *p, unit);
 731
 732                 return (nodename);
 733         }
 734
 735         return (NULL);
 736 }
 737