rfcommd.c (12906B)
1 /* 2 * See LICENSE for copyright details. 3 * 4 * Logic copied from rfcomm.c in bluez. 5 * SDP code from pybluez. 6 * 7 * Copy me if you can. 8 * by 20h 9 */ 10 11 #include <stdio.h> 12 #include <errno.h> 13 #include <fcntl.h> 14 #include <unistd.h> 15 #include <stdlib.h> 16 #include <string.h> 17 #include <getopt.h> 18 #include <signal.h> 19 #include <libgen.h> 20 #include <termios.h> 21 #include <stdint.h> 22 #include <poll.h> 23 #include <stdarg.h> 24 #include <sys/poll.h> 25 #include <sys/param.h> 26 #include <sys/ioctl.h> 27 #include <sys/socket.h> 28 #include <sys/wait.h> 29 30 #include <bluetooth/bluetooth.h> 31 #include <bluetooth/hci.h> 32 #include <bluetooth/hci_lib.h> 33 #include <bluetooth/rfcomm.h> 34 #include <bluetooth/sdp.h> 35 #include <bluetooth/sdp_lib.h> 36 37 #include "arg.h" 38 39 volatile sig_atomic_t __io_canceled = 0; 40 41 int dodebug = 0; 42 43 void 44 debug(char *fmt, ...) 45 { 46 va_list fmtargs; 47 48 if (!dodebug) 49 return; 50 51 va_start(fmtargs, fmt); 52 vfprintf(stderr, fmt, fmtargs); 53 va_end(fmtargs); 54 } 55 56 void 57 sig_hup(int sig) 58 { 59 return; 60 } 61 62 void 63 sig_term(int sig) 64 { 65 __io_canceled = 1; 66 } 67 68 void 69 setup_signals(void) 70 { 71 struct sigaction sa; 72 73 memset(&sa, 0, sizeof(sa)); 74 sa.sa_flags = SA_NOCLDSTOP; 75 sa.sa_handler = SIG_IGN; 76 sigaction(SIGCHLD, &sa, NULL); 77 sigaction(SIGPIPE, &sa, NULL); 78 79 sa.sa_handler = sig_term; 80 sigaction(SIGTERM, &sa, NULL); 81 sigaction(SIGINT, &sa, NULL); 82 83 sa.sa_handler = sig_hup; 84 sigaction(SIGHUP, &sa, NULL); 85 } 86 87 int 88 _adv_available(struct hci_dev_info *di) 89 { 90 uint32_t *flags = &di->flags; 91 int dd; 92 93 if (hci_test_bit(HCI_RAW, &flags) && !bacmp(&di->bdaddr, BDADDR_ANY)) { 94 dd = hci_open_dev(di->dev_id); 95 96 if (dd < 0) 97 return -1; 98 hci_read_bd_addr(dd, &di->bdaddr, 1000); 99 hci_close_dev(dd); 100 } 101 102 return (hci_test_bit(HCI_UP, flags) && 103 hci_test_bit(HCI_RUNNING, flags) && 104 hci_test_bit(HCI_PSCAN, flags) && 105 hci_test_bit(HCI_ISCAN, flags)) != 0 ? 0 : -1; 106 } 107 108 int 109 _any_adv_available(void) 110 { 111 struct hci_dev_list_req *dl = NULL; 112 struct hci_dev_req *dr = NULL; 113 struct hci_dev_info di = {0, }; 114 int result = -1; 115 int ctl = -1; 116 int i; 117 118 if ((ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI)) < 0) 119 return -1; 120 121 if (!(dl = malloc(HCI_MAX_DEV * sizeof(struct hci_dev_req) + 122 sizeof(uint16_t)))) { 123 goto CLEAN_UP_RETURN; 124 } 125 dl->dev_num = HCI_MAX_DEV; 126 dr = dl->dev_req; 127 128 if (ioctl(ctl, HCIGETDEVLIST, (void *)dl) < 0) 129 goto CLEAN_UP_RETURN; 130 131 for (i = 0; i < dl->dev_num; i++) { 132 di.dev_id = (dr+i)->dev_id; 133 if (ioctl(ctl, HCIGETDEVINFO, (void *)&di) < 0) 134 continue; 135 136 if (_adv_available(&di) == 0) { 137 result = 0; 138 goto CLEAN_UP_RETURN; 139 } 140 } 141 142 CLEAN_UP_RETURN: 143 close(ctl); 144 free(dl); 145 146 return result; 147 } 148 149 int 150 adv_available(int sock) 151 { 152 bdaddr_t ba = {{0, }}; 153 struct sockaddr addr = {0, }; 154 int dev_id = -1; 155 socklen_t alen = sizeof(addr); 156 struct sockaddr_rc const *addr_rc = (struct sockaddr_rc const *)&addr; 157 struct hci_dev_info di; 158 159 if (getsockname(sock, &addr, &alen) < 0) 160 return -1; 161 162 ba = addr_rc->rc_bdaddr; 163 164 if (bacmp(&ba, BDADDR_ANY) == 0) { 165 dev_id = -1; 166 } else { 167 dev_id = hci_get_route(&ba); 168 } 169 170 if (dev_id == -1) { 171 return _any_adv_available(); 172 } else { 173 if (hci_devinfo(dev_id, &di)) 174 return -1; 175 return _adv_available(&di); 176 } 177 } 178 179 int 180 str2uuid(char *uuidstr, uuid_t *uuid) 181 { 182 uint32_t uuid_int[4]; 183 int i; 184 char *endptr, buf[9] = { 0 }; 185 186 if (strlen(uuidstr) == 36) { 187 if (uuidstr[8] != '-' && uuidstr[13] != '-' && 188 uuidstr[18] != '-' && uuidstr[23] != '-') { 189 return 1; 190 } 191 192 strncpy(buf, uuidstr, 8); 193 uuid_int[0] = htonl(strtoul(buf, &endptr, 16)); 194 if (endptr != buf+8) 195 return 1; 196 197 strncpy(buf, uuidstr+9, 4); 198 strncpy(buf+4, uuidstr+14, 4); 199 uuid_int[1] = htonl(strtoul(buf, &endptr, 16)); 200 if (endptr != buf+8) 201 return 1; 202 203 strncpy(buf, uuidstr+19, 4); 204 strncpy(buf+4, uuidstr+24, 4); 205 uuid_int[2] = htonl(strtoul(buf, &endptr, 16)); 206 if (endptr != buf+8) 207 return 1; 208 209 strncpy(buf, uuidstr+28, 4); 210 uuid_int[3] = htonl(strtoul(buf, &endptr, 16)); 211 if (endptr != buf+8) 212 return 1; 213 214 if (uuid != NULL) 215 sdp_uuid128_create(uuid, uuid_int); 216 } else if (strlen(uuidstr) == 8) { 217 uuid_int[0] = strtoul(uuidstr, &endptr, 16); 218 if (endptr != uuidstr+8) 219 return 1; 220 if (uuid != NULL) 221 sdp_uuid32_create(uuid, uuid_int[0]); 222 } else if (strlen(uuidstr) == 4) { 223 i = strtol(uuidstr, &endptr, 16); 224 if (endptr != uuidstr+4) 225 return 1; 226 if (uuid != NULL) 227 sdp_uuid16_create(uuid, i); 228 } else { 229 return 1; 230 } 231 232 return 0; 233 } 234 235 int 236 sdp_advertise_service(int sock, char *svcname, 237 char *svcid, int svc_class, int profiles, 238 char *svcprovider, char *svcdescription) 239 { 240 char addrbuf[256]; 241 int res, err = 0; 242 struct sockaddr *sockaddr; 243 uuid_t root_uuid, l2cap_uuid, rfcomm_uuid, svc_class_uuid, 244 svc_uuid; 245 sdp_profile_desc_t *profile_desc; 246 sdp_list_t *l2cap_list = NULL, *rfcomm_list = NULL, *root_list = NULL, 247 *proto_list = NULL, *profile_list = NULL, 248 *svc_class_list = NULL, *access_proto_list = NULL; 249 sdp_data_t *channel = 0; 250 sdp_record_t record; 251 sdp_session_t *session = 0; 252 uint8_t rfcomm_channel; 253 socklen_t addrlen = sizeof(struct sockaddr_rc); 254 255 str2uuid(svcid, &svc_uuid); 256 sdp_uuid16_create(&svc_class_uuid, svc_class); 257 258 memset(addrbuf, 0, sizeof(addrbuf)); 259 260 if (adv_available(sock) < 0) 261 return -1; 262 263 res = getsockname(sock, (struct sockaddr *)addrbuf, &addrlen); 264 if (res < 0) 265 return -1; 266 sockaddr = (struct sockaddr *)addrbuf; 267 268 memset(&record, 0, sizeof(record)); 269 memset(&record.handle, 0xff, sizeof(record.handle)); 270 271 sdp_uuid16_create(&root_uuid, PUBLIC_BROWSE_GROUP); 272 root_list = sdp_list_append(0, &root_uuid); 273 sdp_set_browse_groups(&record, root_list); 274 sdp_uuid16_create(&l2cap_uuid, L2CAP_UUID); 275 l2cap_list = sdp_list_append(0, &l2cap_uuid); 276 proto_list = sdp_list_append(0, l2cap_list); 277 rfcomm_channel = ((struct sockaddr_rc *)sockaddr)->rc_channel; 278 sdp_uuid16_create(&rfcomm_uuid, RFCOMM_UUID); 279 channel = sdp_data_alloc(SDP_UINT8, &rfcomm_channel); 280 rfcomm_list = sdp_list_append(0, &rfcomm_uuid); 281 sdp_list_append(rfcomm_list, channel); 282 sdp_list_append(proto_list, rfcomm_list); 283 access_proto_list = sdp_list_append(0, proto_list); 284 sdp_set_access_protos(&record, access_proto_list); 285 svc_class_list = sdp_list_append(svc_class_list, &svc_class_uuid); 286 sdp_set_service_classes(&record, svc_class_list); 287 288 profile_desc = (sdp_profile_desc_t *)malloc(sizeof(sdp_profile_desc_t)); 289 if (profile_desc == NULL) 290 return -1; 291 sdp_uuid16_create(&profile_desc->uuid, profiles); 292 profile_list = sdp_list_append(profile_list, profile_desc); 293 sdp_set_profile_descs(&record, profile_list); 294 295 sdp_set_info_attr(&record, svcname, svcprovider, svcdescription); 296 sdp_set_service_id(&record, svc_uuid); 297 298 session = sdp_connect(BDADDR_ANY, BDADDR_LOCAL, 0); 299 if (!session) 300 return -1; 301 err = sdp_record_register(session, &record, 0); 302 303 if (channel) 304 sdp_data_free(channel); 305 sdp_list_free(l2cap_list, 0); 306 sdp_list_free(rfcomm_list, 0); 307 sdp_list_free(root_list, 0); 308 sdp_list_free(access_proto_list, 0); 309 sdp_list_free(svc_class_list, 0); 310 sdp_list_free(profile_list, free); 311 312 if (err) 313 return -1; 314 315 return 0; 316 } 317 318 void 319 usage(char *argv0) 320 { 321 fprintf(stderr, "%s [-dhrAESM] [-i hciX|bdaddr] [-L linger] [-c channel] [-f filter cmd] [cmd]\n", 322 basename(argv0)); 323 exit(1); 324 } 325 326 int 327 main(int argc, char *argv[]) 328 { 329 int rfcomm_raw_tty = 0, auth = 0, encryption = 0, 330 secure = 0, master = 0, linger = 0, sk, nsk, fd, lm , try = 30, 331 ctl, rc_channel = 1, filteri, dev; 332 char *argv0, *optarg, dst[18], devname[MAXPATHLEN], *replace, 333 *cmd, *oldcmd, *defaultcmd = NULL, *runcmd; 334 struct sockaddr_rc laddr, raddr; 335 struct rfcomm_dev_req req; 336 struct termios ti; 337 socklen_t alen; 338 bdaddr_t bdaddr; 339 struct linger l; 340 341 char **cmds = NULL; 342 char **filteraddrs = NULL; 343 bdaddr_t **filterbds = NULL; 344 int filtern = 0; 345 346 bacpy(&bdaddr, BDADDR_ANY); 347 348 ARGBEGIN(argv0) { 349 case 'c': 350 rc_channel = atoi(EARGF(usage(argv0))); 351 break; 352 case 'd': 353 dodebug = 1; 354 break; 355 case 'i': 356 optarg = EARGF(usage(argv0)); 357 if (strncmp(optarg, "hci", 3) == 0) { 358 hci_devba(atoi(optarg + 3), &bdaddr); 359 } else { 360 str2ba(optarg, &bdaddr); 361 } 362 break; 363 case 'f': 364 ++filtern; 365 filteraddrs = realloc(filteraddrs, 366 filtern * sizeof(*filteraddrs)); 367 if (filteraddrs == NULL) 368 exit(1); 369 370 cmds = realloc(cmds, filtern * sizeof(*cmds)); 371 if (cmds == NULL) 372 exit(1); 373 374 filterbds = realloc(filterbds, filtern * sizeof(*filterbds)); 375 if (filterbds == NULL) 376 exit(1); 377 378 filteraddrs[filtern-1] = EARGF(usage(argv0)); 379 argv++, argc--; 380 if (argc <= 0) 381 usage(argv0); 382 cmds[filtern-1] = argv[0]; 383 384 filterbds[filtern-1] = malloc(sizeof(*filterbds)); 385 if (filterbds[filtern-1] == NULL) 386 exit(1); 387 str2ba(filteraddrs[filtern-1], filterbds[filtern-1]); 388 break; 389 case 'r': 390 rfcomm_raw_tty = 1; 391 break; 392 case 'A': 393 auth = 1; 394 break; 395 case 'E': 396 encryption = 1; 397 break; 398 case 'S': 399 secure = 1; 400 break; 401 case 'M': 402 master = 1; 403 break; 404 case 'L': 405 linger = atoi(EARGF(usage(argv0))); 406 break; 407 case 'h': 408 default: 409 usage(argv0); 410 } ARGEND; 411 412 if (argc > 0) 413 defaultcmd = argv[0]; 414 if (defaultcmd == NULL && filtern < 0) 415 usage(argv[0]); 416 417 for (filteri = 0; filteri < filtern; filteri++) { 418 ba2str(filterbds[filteri], dst); 419 debug("filter: %s (%s) -> %s\n", 420 filteraddrs[filteri], dst, cmds[filteri]); 421 } 422 debug("defaultcmd: %s\n", defaultcmd); 423 424 setup_signals(); 425 426 ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_RFCOMM); 427 if (ctl < 0) { 428 perror("Can't open RFCOMM control socket"); 429 return 1; 430 } 431 432 laddr.rc_family = AF_BLUETOOTH; 433 bacpy(&laddr.rc_bdaddr, &bdaddr); 434 laddr.rc_channel = rc_channel; 435 436 sk = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); 437 if (sk < 0) { 438 perror("Can't create RFCOMM socket"); 439 return 1; 440 } 441 442 lm = 0; 443 if (master) 444 lm |= RFCOMM_LM_MASTER; 445 if (auth) 446 lm |= RFCOMM_LM_AUTH; 447 if (encryption) 448 lm |= RFCOMM_LM_ENCRYPT; 449 if (secure) 450 lm |= RFCOMM_LM_SECURE; 451 452 if (lm && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &lm, sizeof(lm)) < 0) { 453 perror("Can't set RFCOMM link mode"); 454 close(sk); 455 return 1; 456 } 457 458 if (bind(sk, (struct sockaddr *)&laddr, sizeof(laddr)) < 0) { 459 perror("Can't bind RFCOMM socket"); 460 close(sk); 461 return 1; 462 } 463 464 debug("Waiting for connection on channel %d\n", laddr.rc_channel); 465 466 listen(sk, 10); 467 468 sdp_advertise_service(sk, 469 "SPP Printer", 470 "00001101-0000-1000-8000-00805F9B34FB", 471 SERIAL_PORT_SVCLASS_ID, 472 SERIAL_PORT_PROFILE_ID, 473 "SPP Printer Emulation", 474 "rfcommd"); 475 476 while (!__io_canceled) { 477 alen = sizeof(raddr); 478 nsk = accept(sk, (struct sockaddr *)&raddr, &alen); 479 480 if (fork() != 0) 481 continue; 482 483 ba2str(&raddr.rc_bdaddr, dst); 484 debug("Accept from %s\n", dst); 485 486 for (filteri = 0; filteri < filtern; filteri++) { 487 if (!bacmp(filterbds[filteri], &raddr.rc_bdaddr)) { 488 runcmd = cmds[filteri]; 489 debug("filter found: %s -> %s\n", 490 filteraddrs[filteri], 491 runcmd); 492 break; 493 } 494 } 495 if (filteri >= filtern) { 496 if (defaultcmd != NULL) { 497 debug("running defaultcmd = %s\n", 498 defaultcmd); 499 runcmd = defaultcmd; 500 } else { 501 close(nsk); 502 continue; 503 } 504 } 505 506 alen = sizeof(laddr); 507 if (getsockname(nsk, (struct sockaddr *)&laddr, &alen) < 0) { 508 perror("Can't get RFCOMM socket name"); 509 close(nsk); 510 continue; 511 } 512 513 if (linger) { 514 l.l_onoff = 1; 515 l.l_linger = linger; 516 517 if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) { 518 perror("Can't set linger option"); 519 close(nsk); 520 continue; 521 } 522 } 523 524 memset(&req, 0, sizeof(req)); 525 req.dev_id = -1; 526 req.flags = (1 << RFCOMM_REUSE_DLC) | (1 << RFCOMM_RELEASE_ONHUP); 527 528 bacpy(&req.src, &laddr.rc_bdaddr); 529 bacpy(&req.dst, &raddr.rc_bdaddr); 530 req.channel = raddr.rc_channel; 531 532 dev = ioctl(nsk, RFCOMMCREATEDEV, &req); 533 if (dev < 0) { 534 perror("Can't create RFCOMM TTY"); 535 close(sk); 536 continue; 537 } 538 539 snprintf(devname, MAXPATHLEN - 1, "/dev/rfcomm%d", dev); 540 while ((fd = open(devname, O_RDONLY | O_NOCTTY)) < 0) { 541 if (errno == EACCES) { 542 perror("Can't open RFCOMM device"); 543 goto release; 544 } 545 546 snprintf(devname, MAXPATHLEN - 1, "/dev/bluetooth/rfcomm/%d", dev); 547 if ((fd = open(devname, O_RDONLY | O_NOCTTY)) < 0) { 548 if (try--) { 549 snprintf(devname, MAXPATHLEN - 1, "/dev/rfcomm%d", dev); 550 usleep(100 * 1000); 551 continue; 552 } 553 perror("Can't open RFCOMM device"); 554 goto release; 555 } 556 } 557 558 if (rfcomm_raw_tty) { 559 tcflush(fd, TCIOFLUSH); 560 561 cfmakeraw(&ti); 562 tcsetattr(fd, TCSANOW, &ti); 563 } 564 565 ba2str(&req.dst, dst); 566 debug("Connection from %s to %s\n", dst, devname); 567 568 /* Replace all occurences of '{}' with the rfcomm device path. */ 569 asprintf(&oldcmd, "%s", runcmd); 570 while ((replace = strstr(oldcmd, "{}"))) { 571 replace[0] = '%'; 572 replace[1] = 's'; 573 asprintf(&cmd, oldcmd, devname); 574 free(oldcmd); 575 oldcmd = cmd; 576 } 577 578 debug("Executing %s\n", cmd); 579 580 system(cmd); 581 free(cmd); 582 583 close(fd); 584 close(nsk); 585 release: 586 memset(&req, 0, sizeof(req)); 587 req.dev_id = dev; 588 req.flags = (1 << RFCOMM_HANGUP_NOW); 589 ioctl(ctl, RFCOMMRELEASEDEV, &req); 590 } 591 592 close(sk); 593 594 return 0; 595 } 596