hci.c

这是Linux环境下的蓝牙源代码

		{ "<inquiry with RSSI>",LMP_RSSI_INQ	},	/* Bit 6 */
		{ "<extended SCO>",	LMP_ESCO	},	/* Bit 7 */
		{ NULL }
	},
	{	/* Byte 4 */
		{ "<EV4 packets>",	LMP_EV4		},	/* Bit 0 */
		{ "<EV5 packets>",	LMP_EV5		},	/* Bit 1 */
		{ "<no. 34>",		0x04		},	/* Bit 2 */
		{ "<AFH cap. slave>",	LMP_AFH_CAP_SLV	},	/* Bit 3 */
		{ "<AFH class. slave>",	LMP_AFH_CLS_SLV	},	/* Bit 4 */
		{ "<no. 37>",		0x20		},	/* Bit 5 */
		{ "<no. 38>",		0x40		},	/* Bit 6 */
		{ "<3-slot EDR ACL>",	LMP_EDR_3SLOT	},	/* Bit 7 */
		{ NULL }
	},
	{	/* Byte 5 */
		{ "<5-slot EDR ACL>",	LMP_EDR_5SLOT	},	/* Bit 0 */
		{ "<sniff subrating>",	LMP_SNIFF_SUBR	},	/* Bit 1 */
		{ "<pause encryption>",	LMP_PAUSE_ENC	},	/* Bit 2 */
		{ "<AFH cap. master>",	LMP_AFH_CAP_MST	},	/* Bit 3 */
		{ "<AFH class. master>",LMP_AFH_CLS_MST	},	/* Bit 4 */
		{ "<EDR eSCO 2 Mbps>",	LMP_EDR_ESCO_2M	},	/* Bit 5 */
		{ "<EDR eSCO 3 Mbps>",	LMP_EDR_ESCO_3M	},	/* Bit 6 */
		{ "<3-slot EDR eSCO>",	LMP_EDR_3S_ESCO	},	/* Bit 7 */
		{ NULL }
	},
	{	/* Byte 6 */
		{ "<extended inquiry>",	LMP_EXT_INQ	},	/* Bit 0 */
		{ "<no. 49>",		0x02		},	/* Bit 1 */
		{ "<no. 50>",		0x04		},	/* Bit 2 */
		{ "<simple pairing>",	LMP_SIMPLE_PAIR	},	/* Bit 3 */
		{ "<encapsulated PDU>",	LMP_ENCAPS_PDU	},	/* Bit 4 */
		{ "<err. data report>",	LMP_ERR_DAT_REP	},	/* Bit 5 */
		{ "<non-flush flag>",	LMP_NFLUSH_PKTS	},	/* Bit 6 */
		{ "<no. 55>",		0x80		},	/* Bit 7 */
		{ NULL }
	},
	{	/* Byte 7 */
		{ "<LSTO>",		LMP_LSTO	},	/* Bit 1 */
		{ "<inquiry TX power>",	LMP_INQ_TX_PWR	},	/* Bit 1 */
		{ "<no. 58>",		0x04		},	/* Bit 2 */
		{ "<no. 59>",		0x08		},	/* Bit 3 */
		{ "<no. 60>",		0x10		},	/* Bit 4 */
		{ "<no. 61>",		0x20		},	/* Bit 5 */
		{ "<no. 62>",		0x40		},	/* Bit 6 */
		{ "<extended features>",LMP_EXT_FEAT	},	/* Bit 7 */
		{ NULL }
	},
};

char *lmp_featurestostr(uint8_t *features, char *pref, int width)
{
	char *off, *ptr, *str;
	int i, size = 10;

	for (i = 0; i < 8; i++) {
		hci_map *m = lmp_features_map[i];

		while (m->str) {
			if (m->val & features[i])
				size += strlen(m->str) + (pref ? strlen(pref) : 0) + 1;
			m++;
		}
	}

	str = bt_malloc(size);
	if (!str)
		return NULL;

	ptr = str; *ptr = '\0';

	if (pref)
		ptr += sprintf(ptr, "%s", pref);

	off = ptr;

	for (i = 0; i < 8; i++) {
		hci_map *m = lmp_features_map[i];

		while (m->str) {
			if (m->val & features[i]) {
				if (strlen(off) + strlen(m->str) > width - 1) {
					ptr += sprintf(ptr, "\n%s", pref ? pref : "");
					off = ptr;
				}
				ptr += sprintf(ptr, "%s ", m->str);
			}
			m++;
		}
	}

	return str;
}

/* HCI functions that do not require open device */

int hci_for_each_dev(int flag, int (*func)(int dd, int dev_id, long arg), long arg)
{
	struct hci_dev_list_req *dl;
	struct hci_dev_req *dr;
	int dev_id = -1;
	int i, sk, err = 0;

	sk = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
	if (sk < 0)
		return -1;

	dl = malloc(HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl));
	if (!dl) {
		err = errno;
		goto done;
	}

	memset(dl, 0, HCI_MAX_DEV * sizeof(*dr) + sizeof(*dl));

	dl->dev_num = HCI_MAX_DEV;
	dr = dl->dev_req;

	if (ioctl(sk, HCIGETDEVLIST, (void *) dl) < 0) {
		err = errno;
		goto free;
	}

	for (i = 0; i < dl->dev_num; i++, dr++) {
		if (hci_test_bit(flag, &dr->dev_opt))
			if (!func || func(sk, dr->dev_id, arg)) {
				dev_id = dr->dev_id;
				break;
			}
	}

	if (dev_id < 0)
		err = ENODEV;

free:
	free(dl);

done:
	close(sk);
	errno = err;

	return dev_id;
}

static int __other_bdaddr(int dd, int dev_id, long arg)
{
	struct hci_dev_info di = { dev_id: dev_id };

	if (ioctl(dd, HCIGETDEVINFO, (void *) &di))
		return 0;

	if (hci_test_bit(HCI_RAW, &di.flags))
		return 0;

	return bacmp((bdaddr_t *) arg, &di.bdaddr);
}

static int __same_bdaddr(int dd, int dev_id, long arg)
{
	struct hci_dev_info di = { dev_id: dev_id };

	if (ioctl(dd, HCIGETDEVINFO, (void *) &di))
		return 0;

	return !bacmp((bdaddr_t *) arg, &di.bdaddr);
}

int hci_get_route(bdaddr_t *bdaddr)
{
	return hci_for_each_dev(HCI_UP, __other_bdaddr,
				(long) (bdaddr ? bdaddr : BDADDR_ANY));
}

int hci_devid(const char *str)
{
	bdaddr_t ba;
	int id = -1;

	if (!strncmp(str, "hci", 3) && strlen(str) >= 4) {
		id = atoi(str + 3);
		if (hci_devba(id, &ba) < 0)
			return -1;
	} else {
		errno = ENODEV;
		str2ba(str, &ba);
		id = hci_for_each_dev(HCI_UP, __same_bdaddr, (long) &ba);
	}

	return id;
}

int hci_devinfo(int dev_id, struct hci_dev_info *di)
{
	int dd, err, ret;

	dd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
	if (dd < 0)
		return dd;

	memset(di, 0, sizeof(struct hci_dev_info));

	di->dev_id = dev_id;
	ret = ioctl(dd, HCIGETDEVINFO, (void *) di);

	err = errno;
	close(dd);
	errno = err;

	return ret;
}

int hci_devba(int dev_id, bdaddr_t *bdaddr)
{
	struct hci_dev_info di;

	memset(&di, 0, sizeof(di));

	if (hci_devinfo(dev_id, &di))
		return -1;

	if (!hci_test_bit(HCI_UP, &di.flags)) {
		errno = ENETDOWN;
		return -1;
	}

	bacpy(bdaddr, &di.bdaddr);

	return 0;
}

int hci_inquiry(int dev_id, int len, int nrsp, const uint8_t *lap, inquiry_info **ii, long flags)
{
	struct hci_inquiry_req *ir;
	uint8_t num_rsp = nrsp;
	void *buf;
	int dd, size, err, ret = -1;

	if (nrsp <= 0) {
		num_rsp = 0;
		nrsp = 255;
	}

	if (dev_id < 0) {
		dev_id = hci_get_route(NULL);
		if (dev_id < 0) {
			errno = ENODEV;
			return -1;
		}
	}	

	dd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
	if (dd < 0)
		return dd;

	buf = malloc(sizeof(*ir) + (sizeof(inquiry_info) * (nrsp)));
	if (!buf)
		goto done;

	ir = buf;
	ir->dev_id  = dev_id;
	ir->num_rsp = num_rsp;
	ir->length  = len;
	ir->flags   = flags;

	if (lap) {
		memcpy(ir->lap, lap, 3);
	} else {
		ir->lap[0] = 0x33;
		ir->lap[1] = 0x8b;
		ir->lap[2] = 0x9e;
	}

	ret = ioctl(dd, HCIINQUIRY, (unsigned long) buf);
	if (ret < 0)
		goto free;

	size = sizeof(inquiry_info) * ir->num_rsp;

	if (!*ii)
		*ii = malloc(size);

	if (*ii) {
		memcpy((void *) *ii, buf + sizeof(*ir), size);
		ret = ir->num_rsp;
	} else
		ret = -1;

free:
	free(buf);

done:
	err = errno;
	close(dd);
	errno = err;

	return ret;
}

/* Open HCI device. 
 * Returns device descriptor (dd). */
int hci_open_dev(int dev_id)
{
	struct sockaddr_hci a;
	int dd, err;

	/* Create HCI socket */
	dd = socket(AF_BLUETOOTH, SOCK_RAW, BTPROTO_HCI);
	if (dd < 0)
		return dd;

	/* Bind socket to the HCI device */
	memset(&a, 0, sizeof(a));
	a.hci_family = AF_BLUETOOTH;
	a.hci_dev = dev_id;
	if (bind(dd, (struct sockaddr *) &a, sizeof(a)) < 0)
		goto failed;

	return dd;

failed:
	err = errno;
	close(dd);
	errno = err;

	return -1;
}

int hci_close_dev(int dd)
{
	return close(dd);
}

/* HCI functions that require open device
 * dd - Device descriptor returned by hci_open_dev. */

int hci_send_cmd(int dd, uint16_t ogf, uint16_t ocf, uint8_t plen, void *param)
{
	uint8_t type = HCI_COMMAND_PKT;
	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;
	socklen_t len;
	hci_event_hdr *hdr;
	int err, 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;
		evt_remote_name_req_complete *rn;
		remote_name_req_cp *cp;

		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 (r->event != EVT_CMD_STATUS) {
				if (cs->status) {
					errno = EIO;
					goto failed;
				}
				break;
			}

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

		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;

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

			rn = (void *) ptr;
			cp = r->cparam;

			if (bacmp(&rn->bdaddr, &cp->bdaddr))
				continue;

			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 *bdaddr, 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, bdaddr);
	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_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, (char *) 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;

	memset(&cp, 0, sizeof(cp));
	strncpy((char *) 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_read_remote_name_with_clock_offset(int dd, const bdaddr_t *bdaddr, uint8_t pscan_rep_mode, uint16_t clkoffset, 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, bdaddr);
	cp.pscan_rep_mode = pscan_rep_mode;
	cp.clock_offset   = clkoffset;

	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;