rfcommd.c (12927B)
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 sdp_close(session); 303 304 if (channel) 305 sdp_data_free(channel); 306 sdp_list_free(l2cap_list, 0); 307 sdp_list_free(rfcomm_list, 0); 308 sdp_list_free(root_list, 0); 309 sdp_list_free(access_proto_list, 0); 310 sdp_list_free(svc_class_list, 0); 311 sdp_list_free(profile_list, free); 312 313 if (err) 314 return -1; 315 316 return 0; 317 } 318 319 void 320 usage(char *argv0) 321 { 322 fprintf(stderr, "%s [-dhrAESM] [-i hciX|bdaddr] [-L linger] [-c channel] [-f filter cmd] [cmd]\n", 323 basename(argv0)); 324 exit(1); 325 } 326 327 int 328 main(int argc, char *argv[]) 329 { 330 int rfcomm_raw_tty = 0, auth = 0, encryption = 0, 331 secure = 0, master = 0, linger = 0, sk, nsk, fd, lm , try = 30, 332 ctl, rc_channel = 1, filteri, dev; 333 char *argv0, *optarg, dst[18], devname[MAXPATHLEN], *replace, 334 *cmd, *oldcmd, *defaultcmd = NULL, *runcmd; 335 struct sockaddr_rc laddr, raddr; 336 struct rfcomm_dev_req req; 337 struct termios ti; 338 socklen_t alen; 339 bdaddr_t bdaddr; 340 struct linger l; 341 342 char **cmds = NULL; 343 char **filteraddrs = NULL; 344 bdaddr_t **filterbds = NULL; 345 int filtern = 0; 346 347 bacpy(&bdaddr, BDADDR_ANY); 348 349 ARGBEGIN(argv0) { 350 case 'c': 351 rc_channel = atoi(EARGF(usage(argv0))); 352 break; 353 case 'd': 354 dodebug = 1; 355 break; 356 case 'i': 357 optarg = EARGF(usage(argv0)); 358 if (strncmp(optarg, "hci", 3) == 0) { 359 hci_devba(atoi(optarg + 3), &bdaddr); 360 } else { 361 str2ba(optarg, &bdaddr); 362 } 363 break; 364 case 'f': 365 ++filtern; 366 filteraddrs = realloc(filteraddrs, 367 filtern * sizeof(*filteraddrs)); 368 if (filteraddrs == NULL) 369 exit(1); 370 371 cmds = realloc(cmds, filtern * sizeof(*cmds)); 372 if (cmds == NULL) 373 exit(1); 374 375 filterbds = realloc(filterbds, filtern * sizeof(*filterbds)); 376 if (filterbds == NULL) 377 exit(1); 378 379 filteraddrs[filtern-1] = EARGF(usage(argv0)); 380 argv++, argc--; 381 if (argc <= 0) 382 usage(argv0); 383 cmds[filtern-1] = argv[0]; 384 385 filterbds[filtern-1] = malloc(sizeof(*filterbds)); 386 if (filterbds[filtern-1] == NULL) 387 exit(1); 388 str2ba(filteraddrs[filtern-1], filterbds[filtern-1]); 389 break; 390 case 'r': 391 rfcomm_raw_tty = 1; 392 break; 393 case 'A': 394 auth = 1; 395 break; 396 case 'E': 397 encryption = 1; 398 break; 399 case 'S': 400 secure = 1; 401 break; 402 case 'M': 403 master = 1; 404 break; 405 case 'L': 406 linger = atoi(EARGF(usage(argv0))); 407 break; 408 case 'h': 409 default: 410 usage(argv0); 411 } ARGEND; 412 413 if (argc > 0) 414 defaultcmd = argv[0]; 415 if (defaultcmd == NULL && filtern < 0) 416 usage(argv[0]); 417 418 for (filteri = 0; filteri < filtern; filteri++) { 419 ba2str(filterbds[filteri], dst); 420 debug("filter: %s (%s) -> %s\n", 421 filteraddrs[filteri], dst, cmds[filteri]); 422 } 423 debug("defaultcmd: %s\n", defaultcmd); 424 425 setup_signals(); 426 427 ctl = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_RFCOMM); 428 if (ctl < 0) { 429 perror("Can't open RFCOMM control socket"); 430 return 1; 431 } 432 433 laddr.rc_family = AF_BLUETOOTH; 434 bacpy(&laddr.rc_bdaddr, &bdaddr); 435 laddr.rc_channel = rc_channel; 436 437 sk = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM); 438 if (sk < 0) { 439 perror("Can't create RFCOMM socket"); 440 return 1; 441 } 442 443 lm = 0; 444 if (master) 445 lm |= RFCOMM_LM_MASTER; 446 if (auth) 447 lm |= RFCOMM_LM_AUTH; 448 if (encryption) 449 lm |= RFCOMM_LM_ENCRYPT; 450 if (secure) 451 lm |= RFCOMM_LM_SECURE; 452 453 if (lm && setsockopt(sk, SOL_RFCOMM, RFCOMM_LM, &lm, sizeof(lm)) < 0) { 454 perror("Can't set RFCOMM link mode"); 455 close(sk); 456 return 1; 457 } 458 459 if (bind(sk, (struct sockaddr *)&laddr, sizeof(laddr)) < 0) { 460 perror("Can't bind RFCOMM socket"); 461 close(sk); 462 return 1; 463 } 464 465 debug("Waiting for connection on channel %d\n", laddr.rc_channel); 466 467 listen(sk, 10); 468 469 sdp_advertise_service(sk, 470 "SPP Printer", 471 "00001101-0000-1000-8000-00805F9B34FB", 472 SERIAL_PORT_SVCLASS_ID, 473 SERIAL_PORT_PROFILE_ID, 474 "SPP Printer Emulation", 475 "rfcommd"); 476 477 while (!__io_canceled) { 478 alen = sizeof(raddr); 479 nsk = accept(sk, (struct sockaddr *)&raddr, &alen); 480 481 if (fork() != 0) 482 continue; 483 484 ba2str(&raddr.rc_bdaddr, dst); 485 debug("Accept from %s\n", dst); 486 487 for (filteri = 0; filteri < filtern; filteri++) { 488 if (!bacmp(filterbds[filteri], &raddr.rc_bdaddr)) { 489 runcmd = cmds[filteri]; 490 debug("filter found: %s -> %s\n", 491 filteraddrs[filteri], 492 runcmd); 493 break; 494 } 495 } 496 if (filteri >= filtern) { 497 if (defaultcmd != NULL) { 498 debug("running defaultcmd = %s\n", 499 defaultcmd); 500 runcmd = defaultcmd; 501 } else { 502 close(nsk); 503 continue; 504 } 505 } 506 507 alen = sizeof(laddr); 508 if (getsockname(nsk, (struct sockaddr *)&laddr, &alen) < 0) { 509 perror("Can't get RFCOMM socket name"); 510 close(nsk); 511 continue; 512 } 513 514 if (linger) { 515 l.l_onoff = 1; 516 l.l_linger = linger; 517 518 if (setsockopt(nsk, SOL_SOCKET, SO_LINGER, &l, sizeof(l)) < 0) { 519 perror("Can't set linger option"); 520 close(nsk); 521 continue; 522 } 523 } 524 525 memset(&req, 0, sizeof(req)); 526 req.dev_id = -1; 527 req.flags = (1 << RFCOMM_REUSE_DLC) | (1 << RFCOMM_RELEASE_ONHUP); 528 529 bacpy(&req.src, &laddr.rc_bdaddr); 530 bacpy(&req.dst, &raddr.rc_bdaddr); 531 req.channel = raddr.rc_channel; 532 533 dev = ioctl(nsk, RFCOMMCREATEDEV, &req); 534 if (dev < 0) { 535 perror("Can't create RFCOMM TTY"); 536 close(sk); 537 continue; 538 } 539 540 snprintf(devname, MAXPATHLEN - 1, "/dev/rfcomm%d", dev); 541 while ((fd = open(devname, O_RDONLY | O_NOCTTY)) < 0) { 542 if (errno == EACCES) { 543 perror("Can't open RFCOMM device"); 544 goto release; 545 } 546 547 snprintf(devname, MAXPATHLEN - 1, "/dev/bluetooth/rfcomm/%d", dev); 548 if ((fd = open(devname, O_RDONLY | O_NOCTTY)) < 0) { 549 if (try--) { 550 snprintf(devname, MAXPATHLEN - 1, "/dev/rfcomm%d", dev); 551 usleep(100 * 1000); 552 continue; 553 } 554 perror("Can't open RFCOMM device"); 555 goto release; 556 } 557 } 558 559 if (rfcomm_raw_tty) { 560 tcflush(fd, TCIOFLUSH); 561 562 cfmakeraw(&ti); 563 tcsetattr(fd, TCSANOW, &ti); 564 } 565 566 ba2str(&req.dst, dst); 567 debug("Connection from %s to %s\n", dst, devname); 568 569 /* Replace all occurences of '{}' with the rfcomm device path. */ 570 asprintf(&oldcmd, "%s", runcmd); 571 while ((replace = strstr(oldcmd, "{}"))) { 572 replace[0] = '%'; 573 replace[1] = 's'; 574 asprintf(&cmd, oldcmd, devname); 575 free(oldcmd); 576 oldcmd = cmd; 577 } 578 579 debug("Executing %s\n", cmd); 580 581 system(cmd); 582 free(cmd); 583 584 close(fd); 585 close(nsk); 586 release: 587 memset(&req, 0, sizeof(req)); 588 req.dev_id = dev; 589 req.flags = (1 << RFCOMM_HANGUP_NOW); 590 ioctl(ctl, RFCOMMRELEASEDEV, &req); 591 } 592 593 close(sk); 594 595 return 0; 596 } 597