hci.c

linux下的蓝牙协议栈

	hci_command_hdr hc;
	struct iovec iv[3];
	int ivn;

	hc.opcode = htobs(cmd_opcode_pack(ogf, ocf));
	hc.plen= plen;

	iv[0].iov_base = &type;
	iv[0].iov_len  = 1;
	iv[1].iov_base = &hc;
	iv[1].iov_len  = HCI_COMMAND_HDR_SIZE;
	ivn = 2;

	if (plen) {
		iv[2].iov_base = param;
		iv[2].iov_len  = plen;
		ivn = 3;
	}

	while (writev(dd, iv, ivn) < 0) {
		if (errno == EAGAIN || errno == EINTR)
			continue;
		return -1;
	}
	return 0;
}

int hci_send_req(int dd, struct hci_request *r, int to)
{
	unsigned char buf[HCI_MAX_EVENT_SIZE], *ptr;
	uint16_t opcode = htobs(cmd_opcode_pack(r->ogf, r->ocf));
	struct hci_filter nf, of;
	hci_event_hdr *hdr;
	int err, len, try;

	len = sizeof(of);
	if (getsockopt(dd, SOL_HCI, HCI_FILTER, &of, &len) < 0)
		return -1;

	hci_filter_clear(&nf);
	hci_filter_set_ptype(HCI_EVENT_PKT,  &nf);
	hci_filter_set_event(EVT_CMD_STATUS, &nf);
	hci_filter_set_event(EVT_CMD_COMPLETE, &nf);
	hci_filter_set_event(r->event, &nf);
	hci_filter_set_opcode(opcode, &nf);
	if (setsockopt(dd, SOL_HCI, HCI_FILTER, &nf, sizeof(nf)) < 0)
		return -1;

	if (hci_send_cmd(dd, r->ogf, r->ocf, r->clen, r->cparam) < 0)
		goto failed;

	try = 10;
	while (try--) {
		evt_cmd_complete *cc;
		evt_cmd_status   *cs;

		if (to) {
			struct pollfd p;
			int n;

			p.fd = dd; p.events = POLLIN;
			while ((n = poll(&p, 1, to)) < 0) {
				if (errno == EAGAIN || errno == EINTR)
					continue;
				goto failed;
			}

			if (!n) {
				errno = ETIMEDOUT;
				goto failed;
			}

			to -= 10;
			if (to < 0) to = 0;

		}

		while ((len = read(dd, buf, sizeof(buf))) < 0) {
			if (errno == EAGAIN || errno == EINTR)
				continue;
			goto failed;
		}

		hdr = (void *) (buf + 1);
		ptr = buf + (1 + HCI_EVENT_HDR_SIZE);
		len -= (1 + HCI_EVENT_HDR_SIZE);

		switch (hdr->evt) {
		case EVT_CMD_STATUS:
			cs = (void *) ptr;

			if (cs->opcode != opcode)
				continue;

			if (cs->status) {
				errno = EIO;
				goto failed;
			}
			break;

		case EVT_CMD_COMPLETE:
			cc = (void *) ptr;

			if (cc->opcode != opcode)
				continue;

			ptr += EVT_CMD_COMPLETE_SIZE;
			len -= EVT_CMD_COMPLETE_SIZE;

			r->rlen = MIN(len, r->rlen);
			memcpy(r->rparam, ptr, r->rlen);
			goto done;

		default:
			if (hdr->evt != r->event)
				break;

			r->rlen = MIN(len, r->rlen);
			memcpy(r->rparam, ptr, r->rlen);
			goto done;
		}
	}
	errno = ETIMEDOUT;

failed:
	err = errno;
	setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
	errno = err;
	return -1;

done:
	setsockopt(dd, SOL_HCI, HCI_FILTER, &of, sizeof(of));
	return 0;
}

int hci_create_connection(int dd, const bdaddr_t *ba, uint16_t ptype, uint16_t clkoffset, uint8_t rswitch, uint16_t *handle, int to)
{
	evt_conn_complete rp;
	create_conn_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	bacpy(&cp.bdaddr, ba);
	cp.pkt_type       = ptype;
	cp.pscan_rep_mode = 0x02;
	cp.clock_offset   = clkoffset;
	cp.role_switch    = rswitch;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_CREATE_CONN;
	rq.event  = EVT_CONN_COMPLETE;
	rq.cparam = &cp;
	rq.clen   = CREATE_CONN_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_CONN_COMPLETE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	*handle = rp.handle;
	return 0;
}

int hci_disconnect(int dd, uint16_t handle, uint8_t reason, int to)
{
	evt_disconn_complete rp;
	disconnect_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	cp.handle = handle;
	cp.reason = reason;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_DISCONNECT;
	rq.event  = EVT_DISCONN_COMPLETE;
	rq.cparam = &cp;
	rq.clen   = DISCONNECT_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_DISCONN_COMPLETE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}
	return 0;
}

int hci_local_name(int dd, int len, char *name, int to)
{
	return hci_read_local_name(dd, len, name, to);
}

int hci_read_local_name(int dd, int len, char *name, int to)
{
	read_local_name_rp rp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_READ_LOCAL_NAME;
	rq.rparam = &rp;
	rq.rlen   = READ_LOCAL_NAME_RP_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	rp.name[247] = '\0';
	strncpy(name, rp.name, len);
	return 0;
}

int hci_write_local_name(int dd, const char *name, int to)
{
	change_local_name_cp cp;
	struct hci_request rq;

	strncpy(cp.name, name, sizeof(cp.name));
	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_CHANGE_LOCAL_NAME;
	rq.cparam = &cp;
	rq.clen   = CHANGE_LOCAL_NAME_CP_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;
	return 0;
}

int hci_remote_name(int dd, const bdaddr_t *ba, int len, char *name, int to)
{
	return hci_read_remote_name(dd, ba, len, name, to);
}

int hci_read_remote_name(int dd, const bdaddr_t *ba, int len, char *name, int to)
{
	evt_remote_name_req_complete rn;
	remote_name_req_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	bacpy(&cp.bdaddr, ba);
	cp.pscan_rep_mode = 0x02;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_REMOTE_NAME_REQ;
	rq.cparam = &cp;
	rq.clen   = REMOTE_NAME_REQ_CP_SIZE;
	rq.event  = EVT_REMOTE_NAME_REQ_COMPLETE;
	rq.rparam = &rn;
	rq.rlen   = EVT_REMOTE_NAME_REQ_COMPLETE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rn.status) {
		errno = EIO;
		return -1;
	}

	rn.name[247] = '\0';
	strncpy(name, rn.name, len);
	return 0;
}

int hci_read_remote_features(int dd, uint16_t handle, uint8_t *features, int to)
{
	evt_read_remote_features_complete rp;
	read_remote_features_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	cp.handle = handle;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_READ_REMOTE_FEATURES;
	rq.event  = EVT_READ_REMOTE_FEATURES_COMPLETE;
	rq.cparam = &cp;
	rq.clen   = READ_REMOTE_FEATURES_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_READ_REMOTE_FEATURES_COMPLETE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	memcpy(features, rp.features, 8);
	return 0;
}

int hci_read_remote_version(int dd, uint16_t handle, struct hci_version *ver, int to)
{
	evt_read_remote_version_complete rp;
	read_remote_version_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	cp.handle = handle;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_READ_REMOTE_VERSION;
	rq.event  = EVT_READ_REMOTE_VERSION_COMPLETE;
	rq.cparam = &cp;
	rq.clen   = READ_REMOTE_VERSION_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_READ_REMOTE_VERSION_COMPLETE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	ver->manufacturer = btohs(rp.manufacturer);
	ver->lmp_ver      = rp.lmp_ver;
	ver->lmp_subver   = btohs(rp.lmp_subver);
	return 0;
}

int hci_read_clock_offset(int dd, uint16_t handle, uint16_t *clkoffset, int to)
{
	evt_read_clock_offset_complete rp;
	read_clock_offset_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	cp.handle = handle;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_READ_CLOCK_OFFSET;
	rq.event  = EVT_READ_CLOCK_OFFSET_COMPLETE;
	rq.cparam = &cp;
	rq.clen   = READ_CLOCK_OFFSET_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_READ_CLOCK_OFFSET_COMPLETE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	*clkoffset = rp.clock_offset;
	return 0;
}

int hci_read_local_version(int dd, struct hci_version *ver, int to)
{
	read_local_version_rp rp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_INFO_PARAM;
	rq.ocf    = OCF_READ_LOCAL_VERSION;
	rq.rparam = &rp;
	rq.rlen   = READ_LOCAL_VERSION_RP_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	ver->manufacturer = btohs(rp.manufacturer);
	ver->hci_ver      = rp.hci_ver;
	ver->hci_rev      = btohs(rp.hci_rev);
	ver->lmp_ver      = rp.lmp_ver;
	ver->lmp_subver   = btohs(rp.lmp_subver);
	return 0;
}

int hci_read_class_of_dev(int dd, uint8_t *cls, int to)
{
	read_class_of_dev_rp rp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_READ_CLASS_OF_DEV;
	rq.rparam = &rp;
	rq.rlen   = READ_CLASS_OF_DEV_RP_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	memcpy(cls, rp.dev_class, 3);
	return 0;
}

int hci_write_class_of_dev(int dd, uint32_t cls, int to)
{
	write_class_of_dev_cp cp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	cp.dev_class[0] = cls & 0xff;
	cp.dev_class[1] = (cls >> 8) & 0xff;
	cp.dev_class[2] = (cls >> 16) & 0xff;
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_WRITE_CLASS_OF_DEV;
	rq.cparam = &cp;
	rq.clen   = WRITE_CLASS_OF_DEV_CP_SIZE;
	return hci_send_req(dd, &rq, to);
}

int hci_read_voice_setting(int dd, uint16_t *vs, int to)
{
	read_voice_setting_rp rp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_READ_VOICE_SETTING;
	rq.rparam = &rp;
	rq.rlen   = READ_VOICE_SETTING_RP_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	*vs = rp.voice_setting;
	return 0;
}

int hci_write_voice_setting(int dd, uint16_t vs, int to)
{
	write_voice_setting_cp cp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	cp.voice_setting = vs;
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_WRITE_VOICE_SETTING;
	rq.cparam = &cp;
	rq.clen   = WRITE_VOICE_SETTING_CP_SIZE;
	return hci_send_req(dd, &rq, to);
}

int hci_read_current_iac_lap(int dd, uint8_t *num_iac, uint8_t *lap, int to)
{
	read_current_iac_lap_rp rp;
	struct hci_request rq;

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_READ_CURRENT_IAC_LAP;
	rq.rparam = &rp;
	rq.rlen   = READ_CURRENT_IAC_LAP_RP_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	*num_iac = rp.num_current_iac;
	memcpy(lap, rp.lap, rp.num_current_iac * 3);
	return 0;
}

int hci_write_current_iac_lap(int dd, uint8_t num_iac, uint8_t *lap, int to)
{
	write_current_iac_lap_cp cp;
	struct hci_request rq;

	memset(&cp, 0, sizeof(cp));
	cp.num_current_iac = num_iac;
	memcpy(&cp.lap, lap, num_iac * 3);

	memset(&rq, 0, sizeof(rq));
	rq.ogf    = OGF_HOST_CTL;
	rq.ocf    = OCF_WRITE_CURRENT_IAC_LAP;
	rq.cparam = &cp;
	rq.clen   = WRITE_CURRENT_IAC_LAP_CP_SIZE;
	return hci_send_req(dd, &rq, to);
}

int hci_authenticate_link(int dd, uint16_t handle, int to)
{
	auth_requested_cp cp;
	evt_auth_complete rp;
	struct hci_request rq;

	cp.handle = handle;
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_AUTH_REQUESTED;
	rq.cparam = &cp;
	rq.clen   = AUTH_REQUESTED_CP_SIZE;
	rq.rparam = &rp;
	rq.event  = EVT_AUTH_COMPLETE;
	rq.rlen   = EVT_AUTH_COMPLETE_SIZE;
	if (hci_send_req(dd, &rq, to) < 0)
		return -1;
	if (rp.status) {
		errno = EIO;
		return -1;
	}
	return 0;
}

int hci_encrypt_link(int dd, uint16_t handle, int on, int to)
{
	set_conn_encrypt_cp cp;
	evt_encrypt_change rp;
	struct hci_request rq;

	cp.handle  = handle;
	cp.encrypt = on;
	rq.ogf    = OGF_LINK_CTL;
	rq.ocf    = OCF_SET_CONN_ENCRYPT;
	rq.cparam = &cp;
	rq.clen   = SET_CONN_ENCRYPT_CP_SIZE;
	rq.event  = EVT_ENCRYPT_CHANGE;
	rq.rlen   = EVT_ENCRYPT_CHANGE_SIZE;
	rq.rparam = &rp;
	if (hci_send_req(dd, &rq, to) < 0)
		return -1;
	if (rp.status) {
		errno = EIO;
		return -1;
	}
	return 0;
}

int hci_switch_role(int dd, bdaddr_t peer, int role, int to)
{
	switch_role_cp cp;
	evt_role_change rp;
	struct hci_request rq;

	cp.bdaddr = peer;
	cp.role   = role;
	rq.ogf    = OGF_LINK_POLICY;
	rq.ocf    = OCF_SWITCH_ROLE;
	rq.cparam = &cp;
	rq.clen   = SWITCH_ROLE_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_ROLE_CHANGE_SIZE;
	rq.event  = EVT_ROLE_CHANGE;
	if (hci_send_req(dd, &rq, to) < 0)
		return -1;
	if (rp.status) {
		errno = EIO;
		return -1;
	}
	return 0;
}

int hci_park_mode(int dd, uint16_t handle, uint16_t max_interval, uint16_t min_interval, int to)
{
	park_mode_cp cp;
	evt_mode_change rp;
	struct hci_request rq;

	memset(&cp, 0, sizeof (cp));
	cp.handle       = handle;
	cp.max_interval = max_interval;
	cp.min_interval = min_interval;

	memset(&rq, 0, sizeof (rq));
	rq.ogf    = OGF_LINK_POLICY;
	rq.ocf    = OCF_PARK_MODE;
	rq.event  = EVT_MODE_CHANGE;
	rq.cparam = &cp;
	rq.clen   = PARK_MODE_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_MODE_CHANGE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	return 0;
}

int hci_exit_park_mode(int dd, uint16_t handle, int to)
{
	exit_park_mode_cp cp;
	evt_mode_change rp;
	struct hci_request rq;

	memset(&cp, 0, sizeof (cp));
	cp.handle = handle;

	memset (&rq, 0, sizeof (rq));
	rq.ogf    = OGF_LINK_POLICY;
	rq.ocf    = OCF_EXIT_PARK_MODE;
	rq.event  = EVT_MODE_CHANGE;
	rq.cparam = &cp;
	rq.clen   = EXIT_PARK_MODE_CP_SIZE;
	rq.rparam = &rp;
	rq.rlen   = EVT_MODE_CHANGE_SIZE;

	if (hci_send_req(dd, &rq, to) < 0)
		return -1;

	if (rp.status) {
		errno = EIO;
		return -1;
	}

	return 0;
}