hci_core.c

嵌入式系统设计与实例开发实验教材二源码 多线程应用程序设计 串行端口程序设计 AD接口实验 CAN总线通信实验 GPS通信实验 Linux内核移植与编译实验 IC卡读写实验 SD驱动使

		return -EFAULT;

	if (!(hdev = hci_dev_get(dr.dev_id)))
		return -ENODEV;

	ret = hci_request(hdev, hci_auth_req, dr.dev_opt, HCI_INIT_TIMEOUT);

	hci_dev_put(hdev);

	return ret;
}

int hci_dev_setscan(unsigned long arg)
{
	struct hci_dev *hdev;
	struct hci_dev_req dr;
	int ret = 0;

	if (copy_from_user(&dr, (void *) arg, sizeof(dr)))
		return -EFAULT;

	if (!(hdev = hci_dev_get(dr.dev_id)))
		return -ENODEV;

	ret = hci_request(hdev, hci_scan_req, dr.dev_opt, HCI_INIT_TIMEOUT);

	hci_dev_put(hdev);

	return ret;
}

int hci_dev_setptype(unsigned long arg)
{
	struct hci_dev *hdev;
	struct hci_dev_req dr;
	int ret = 0;

	if (copy_from_user(&dr, (void *) arg, sizeof(dr)))
		return -EFAULT;

	if (!(hdev = hci_dev_get(dr.dev_id)))
		return -ENODEV;

	hdev->pkt_type = (__u16) dr.dev_opt;

	hci_dev_put(hdev);

	return ret;
}

int hci_dev_list(unsigned long arg)
{
	struct hci_dev_list_req *dl;
	struct hci_dev_req *dr;
	struct hci_dev *hdev;
	int i, n, size;
	__u16 dev_num;

	if (get_user(dev_num, (__u16 *) arg))
		return -EFAULT;

	/* Avoid long loop, overflow */
	if (dev_num > 2048)
		return -EINVAL;
	
	size = dev_num * sizeof(struct hci_dev_req) + sizeof(__u16);

	if (verify_area(VERIFY_WRITE, (void *) arg, size))
		return -EFAULT;

	if (!(dl = kmalloc(size, GFP_KERNEL)))
		return -ENOMEM;
	dr = dl->dev_req;

	spin_lock_bh(&hdev_list_lock);
	for (i = 0, n = 0; i < HCI_MAX_DEV && n < dev_num; i++) {
		if ((hdev = hdev_list[i])) {
			(dr + n)->dev_id  = hdev->id;
			(dr + n)->dev_opt = hdev->flags;
			n++;
		}
	}
	spin_unlock_bh(&hdev_list_lock);

	dl->dev_num = n;
	size = n * sizeof(struct hci_dev_req) + sizeof(__u16);

	copy_to_user((void *) arg, dl, size);

	return 0;
}

int hci_dev_info(unsigned long arg)
{
	struct hci_dev *hdev;
	struct hci_dev_info di;
	int err = 0;

	if (copy_from_user(&di, (void *) arg, sizeof(di)))
		return -EFAULT;

	if (!(hdev = hci_dev_get(di.dev_id)))
		return -ENODEV;

	strcpy(di.name, hdev->name);
	di.bdaddr   = hdev->bdaddr;
	di.type     = hdev->type;
	di.flags    = hdev->flags;
	di.pkt_type = hdev->pkt_type;
	di.acl_mtu  = hdev->acl_mtu;
	di.acl_max  = hdev->acl_max;
	di.sco_mtu  = hdev->sco_mtu;
	di.sco_max  = hdev->sco_max;

	memcpy(&di.stat, &hdev->stat, sizeof(di.stat));
	memcpy(&di.features, &hdev->features, sizeof(di.features));

	if (copy_to_user((void *) arg, &di, sizeof(di)))
		err = -EFAULT;

	hci_dev_put(hdev);

	return err;
}

__u32 hci_dev_setmode(struct hci_dev *hdev, __u32 mode)
{
	__u32 omode = hdev->flags & HCI_MODE_MASK;

	hdev->flags &= ~HCI_MODE_MASK;
	hdev->flags |= (mode & HCI_MODE_MASK);

	return omode;
}

__u32 hci_dev_getmode(struct hci_dev *hdev)
{
	return hdev->flags & HCI_MODE_MASK;
}

int hci_conn_list(unsigned long arg)
{
	struct hci_conn_list_req req, *cl;
	struct hci_conn_info *ci;
	struct hci_dev *hdev;
	struct list_head *p;
	int n = 0, size;

	if (copy_from_user(&req, (void *) arg, sizeof(req)))
		return -EFAULT;

	if (!(hdev = hci_dev_get(req.dev_id)))
		return -ENODEV;

	/* Set a limit to avoid overlong loops, and also numeric overflow - AC */
	if(req.conn_num < 2048)
		return -EINVAL;
	
	size = req.conn_num * sizeof(struct hci_conn_info) + sizeof(req);

	if (!(cl = kmalloc(size, GFP_KERNEL)))
		return -ENOMEM;
	ci = cl->conn_info;

	local_bh_disable();
	conn_hash_lock(&hdev->conn_hash);
	list_for_each(p, &hdev->conn_hash.list) {
		register struct hci_conn *c;
		c = list_entry(p, struct hci_conn, list);

		(ci + n)->handle = c->handle;
		bacpy(&(ci + n)->bdaddr, &c->dst);
		n++;
	}
	conn_hash_unlock(&hdev->conn_hash);
	local_bh_enable();

	cl->dev_id = hdev->id;
	cl->conn_num = n;
	size = n * sizeof(struct hci_conn_info) + sizeof(req);

	hci_dev_put(hdev);

	if(copy_to_user((void *) arg, cl, size))
		return -EFAULT;
	return 0;
}

int hci_inquiry(unsigned long arg)
{
	struct inquiry_cache *cache;
	struct hci_inquiry_req ir;
	struct hci_dev *hdev;
	int err = 0, do_inquiry = 0;
	long timeo;
	__u8 *buf, *ptr;

	ptr = (void *) arg;
	if (copy_from_user(&ir, ptr, sizeof(ir)))
		return -EFAULT;

	if (!(hdev = hci_dev_get(ir.dev_id)))
		return -ENODEV;

	cache = &hdev->inq_cache;

	inquiry_cache_lock(cache);
	if (inquiry_cache_age(cache) > INQUIRY_CACHE_AGE_MAX || ir.flags & IREQ_CACHE_FLUSH) {
		inquiry_cache_flush(cache);
		do_inquiry = 1;
	}
	inquiry_cache_unlock(cache);

	/* Limit inquiry time, also avoid overflows */

	if(ir.length > 2048 || ir.num_rsp > 2048)
	{
		err = -EINVAL;
		goto done;
	}

	timeo = ir.length * 2 * HZ;
	if (do_inquiry && (err = hci_request(hdev, hci_inq_req, (unsigned long)&ir, timeo)) < 0)
		goto done;

	/* cache_dump can't sleep. Therefore we allocate temp buffer and then
	 * copy it to the user space.
	 */
	if (!(buf = kmalloc(sizeof(inquiry_info) * ir.num_rsp, GFP_KERNEL))) {
		err = -ENOMEM;
		goto done;
	}
	ir.num_rsp = inquiry_cache_dump(cache, ir.num_rsp, buf);

	DBG("num_rsp %d", ir.num_rsp);

	if (!verify_area(VERIFY_WRITE, ptr, sizeof(ir) + (sizeof(inquiry_info) * ir.num_rsp))) {
		copy_to_user(ptr, &ir, sizeof(ir));
		ptr += sizeof(ir);
	        copy_to_user(ptr, buf, sizeof(inquiry_info) * ir.num_rsp);
	} else 
		err = -EFAULT;

	kfree(buf);

done:
	hci_dev_put(hdev);

	return err;
}

/* Interface to HCI drivers */

/* Register HCI device */
int hci_register_dev(struct hci_dev *hdev)
{
	int i;

	DBG("%p name %s type %d", hdev, hdev->name, hdev->type);

	/* Find free slot */
	spin_lock_bh(&hdev_list_lock);
	for (i = 0; i < HCI_MAX_DEV; i++) {
		if (!hdev_list[i]) {
			hdev_list[i] = hdev;

			sprintf(hdev->name, "hci%d", i);
			atomic_set(&hdev->refcnt, 0);
			hdev->id    = i;
			hdev->flags = HCI_NORMAL;

			hdev->pkt_type = (HCI_DM1 | HCI_DH1);

			tasklet_init(&hdev->cmd_task, hci_cmd_task, (unsigned long) hdev);
			tasklet_init(&hdev->rx_task, hci_rx_task, (unsigned long) hdev);
			tasklet_init(&hdev->tx_task, hci_tx_task, (unsigned long) hdev);

			skb_queue_head_init(&hdev->rx_q);
			skb_queue_head_init(&hdev->cmd_q);
			skb_queue_head_init(&hdev->raw_q);

			init_waitqueue_head(&hdev->req_wait_q);
			init_MUTEX(&hdev->req_lock);

			inquiry_cache_init(&hdev->inq_cache);

			conn_hash_init(&hdev->conn_hash);

			memset(&hdev->stat, 0, sizeof(struct hci_dev_stats));

			hci_notify(hdev, HCI_DEV_REG);

			MOD_INC_USE_COUNT;
			break;
		}
	}
	spin_unlock_bh(&hdev_list_lock);

	return (i == HCI_MAX_DEV) ? -1 : i;
}

/* Unregister HCI device */
int hci_unregister_dev(struct hci_dev *hdev)
{
	int i;

	DBG("%p name %s type %d", hdev, hdev->name, hdev->type);

	if (hdev->flags & HCI_UP)
		hci_dev_close(hdev->id);

	/* Find device slot */
	spin_lock(&hdev_list_lock);
	for (i = 0; i < HCI_MAX_DEV; i++) {
		if (hdev_list[i] == hdev) {
			hdev_list[i] = NULL;
			MOD_DEC_USE_COUNT;
			break;
		}
	}
	spin_unlock(&hdev_list_lock);

	hci_notify(hdev, HCI_DEV_UNREG);

	/* Sleep while device is in use */
	while (atomic_read(&hdev->refcnt)) {
		int sleep_cnt = 100;

		DBG("%s sleeping on lock %d", hdev->name, atomic_read(&hdev->refcnt));

		sleep_on_timeout(&hdev->req_wait_q, HZ*10);
		if (!(--sleep_cnt))
			break;
	}

	return 0;
}

/* Interface to upper protocols */

/* Register/Unregister protocols.
 * hci_task_lock is used to ensure that no tasks are running.
 */
int hci_register_proto(struct hci_proto *hproto)
{
	int err = 0;

	DBG("%p name %s", hproto, hproto->name);

	if (hproto->id >= HCI_MAX_PROTO)
		return -EINVAL;

	write_lock_bh(&hci_task_lock);

	if (!hproto_list[hproto->id])
		hproto_list[hproto->id] = hproto;
	else
		err = -1;

	write_unlock_bh(&hci_task_lock);

	return err;
}

int hci_unregister_proto(struct hci_proto *hproto)
{
	int err = 0;

	DBG("%p name %s", hproto, hproto->name);

	if (hproto->id > HCI_MAX_PROTO)
		return -EINVAL;

	write_lock_bh(&hci_task_lock);

	if (hproto_list[hproto->id])
		hproto_list[hproto->id] = NULL;
	else
		err = -ENOENT;

	write_unlock_bh(&hci_task_lock);

	return err;
}

static int hci_send_frame(struct sk_buff *skb)
{
	struct hci_dev *hdev = (struct hci_dev *) skb->dev;

	if (!hdev) {
		kfree_skb(skb);
		return -ENODEV;
	}

	DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);

	if (hdev->flags & HCI_SOCK)
		hci_send_to_sock(hdev, skb);

	/* Get rid of skb owner, prior to sending to the driver. */
	skb_orphan(skb);

	return hdev->send(skb);
}

/* Connection scheduler */
static inline struct hci_conn *hci_low_sent(struct hci_dev *hdev, __u8 type, int *quote)
{
	struct conn_hash *h = &hdev->conn_hash;
	struct hci_conn *conn = NULL;
	int num = 0, min = 0xffff;
        struct list_head *p;

	conn_hash_lock(h);
	list_for_each(p, &h->list) {
		register struct hci_conn *c;

		c = list_entry(p, struct hci_conn, list);

		if (c->type != type || skb_queue_empty(&c->data_q))
			continue;
		num++;

		if (c->sent < min) {
			min  = c->sent;
			conn = c;
		}
	}
	conn_hash_unlock(h);

	if (conn) {
		int q = hdev->acl_cnt / num;
		*quote = q ? q : 1;
	} else
		*quote = 0;

	DBG("conn %p quote %d", conn, *quote);

	return conn;
}

static inline void hci_sched_acl(struct hci_dev *hdev)
{
	struct hci_conn *conn;
	struct sk_buff *skb;
	int quote;

	DBG("%s", hdev->name);

	while (hdev->acl_cnt && (conn = hci_low_sent(hdev, ACL_LINK, &quote))) {
		while (quote && (skb = skb_dequeue(&conn->data_q))) {
			DBG("skb %p len %d", skb, skb->len);

			hci_send_frame(skb);

			conn->sent++;
			hdev->acl_cnt--;
			quote--;
		}
	}
}

/* Schedule SCO */
static inline void hci_sched_sco(struct hci_dev *hdev)
{
	/* FIXME: For now we queue SCO packets to the raw queue 

		while (hdev->sco_cnt && (skb = skb_dequeue(&conn->data_q))) {
			hci_send_frame(skb);
			conn->sco_sent++;
			hdev->sco_cnt--;
		}
	*/
}

/* Get data from the previously sent command */
static void * hci_sent_cmd_data(struct hci_dev *hdev, __u16 ogf, __u16 ocf)
{
	hci_command_hdr *hc;

	if (!hdev->sent_cmd)
		return NULL;

	hc = (void *) hdev->sent_cmd->data;

	if (hc->opcode != __cpu_to_le16(cmd_opcode_pack(ogf, ocf)))
		return NULL;

	DBG("%s ogf 0x%x ocf 0x%x", hdev->name, ogf, ocf);

	return hdev->sent_cmd->data + HCI_COMMAND_HDR_SIZE;
}

/* Send raw HCI frame */
int hci_send_raw(struct sk_buff *skb)
{
	struct hci_dev *hdev = (struct hci_dev *) skb->dev;

	if (!hdev) {
		kfree_skb(skb);
		return -ENODEV;
	}

	DBG("%s type %d len %d", hdev->name, skb->pkt_type, skb->len);

	if (hdev->flags & HCI_NORMAL) {
		/* Queue frame according it's type */
		switch (skb->pkt_type) {
		case HCI_COMMAND_PKT:
			skb_queue_tail(&hdev->cmd_q, skb);
			hci_sched_cmd(hdev);
			return 0;

		case HCI_ACLDATA_PKT:
		case HCI_SCODATA_PKT:
			/* FIXME:
		 	 * Check header here and queue to apropriate connection.
		 	 */
			break;
		}
	}

	skb_queue_tail(&hdev->raw_q, skb);
	hci_sched_tx(hdev);
	return 0;
}

/* Send HCI command */
int hci_send_cmd(struct hci_dev *hdev, __u16 ogf, __u16 ocf, __u32 plen, void *param)
{
	int len = HCI_COMMAND_HDR_SIZE + plen;
	hci_command_hdr *hc;
	struct sk_buff *skb;

	DBG("%s ogf 0x%x ocf 0x%x plen %d", hdev->name, ogf, ocf, plen);

	if (!(skb = bluez_skb_alloc(len, GFP_ATOMIC))) {
		ERR("%s Can't allocate memory for HCI command", hdev->name);
		return -ENOMEM;
	}
	
	hc = (hci_command_hdr *) skb_put(skb, HCI_COMMAND_HDR_SIZE);
	hc->opcode = __cpu_to_le16(cmd_opcode_pack(ogf, ocf));
	hc->plen   = plen;

	if (plen)
		memcpy(skb_put(skb, plen), param, plen);

	DBG("skb len %d", skb->len);

	skb->pkt_type = HCI_COMMAND_PKT;
	skb->dev = (void *) hdev;
	skb_queue_tail(&hdev->cmd_q, skb);
	hci_sched_cmd(hdev);

	return 0;
}

/* Send ACL data */
static void hci_add_acl_hdr(struct sk_buff *skb, __u16 handle, __u16 flags)
{
	int len = skb->len;	
	hci_acl_hdr *ah;

	ah = (hci_acl_hdr *) skb_push(skb, HCI_ACL_HDR_SIZE);
	ah->handle = __cpu_to_le16(acl_handle_pack(handle, flags));
	ah->dlen   = __cpu_to_le16(len);

	skb->h.raw = (void *) ah;
}

int hci_send_acl(struct hci_conn *conn, struct sk_buff *skb, __u16 flags)
{
	struct hci_dev *hdev = conn->hdev;
	struct sk_buff *list;

	DBG("%s conn %p flags 0x%x", hdev->name, conn, flags);

	skb->dev = (void *) hdev;
	skb->pkt_type = HCI_ACLDATA_PKT;
	hci_add_acl_hdr(skb, conn->handle, flags | ACL_START);

	if (!(list = skb_shinfo(skb)->frag_list)) {
		/* Non fragmented */
		DBG("%s nonfrag skb %p len %d", hdev->name, skb, skb->len);
		
		skb_queue_tail(&conn->data_q, skb);
	} else {
		/* Fragmented */
		DBG("%s frag %p len %d", hdev->name, skb, skb->len);

		skb_shinfo(skb)->frag_list = NULL;

		/* Queue all fragments atomically */
		spin_lock_bh(&conn->data_q.lock);

		__skb_queue_tail(&conn->data_q, skb);
		do {
			skb = list; list = list->next;
			
			skb->dev = (void *) hdev;
			skb->pkt_type = HCI_ACLDATA_PKT;
			hci_add_acl_hdr(skb, conn->handle, flags | ACL_CONT);
		
			DBG("%s frag %p len %d", hdev->name, skb, skb->len);

			__skb_queue_tail(&conn->data_q, skb);
		} while (list);

		spin_unlock_bh(&conn->data_q.lock);
	}
		
	hci_sched_tx(hdev);
	return 0;
}

/* Send SCO data */
int hci_send_sco(struct hci_conn *conn, struct sk_buff *skb)
{
	struct hci_dev *hdev = conn->hdev;
	hci_sco_hdr hs;

	DBG("%s len %d", hdev->name, skb->len);

	if (skb->len > hdev->sco_mtu) {
		kfree_skb(skb);
		return -EINVAL;
	}

	hs.handle = __cpu_to_le16(conn->handle);
	hs.dlen   = skb->len;

	skb->h.raw = skb_push(skb, HCI_SCO_HDR_SIZE);
	memcpy(skb->h.raw, &hs, HCI_SCO_HDR_SIZE);

	skb->dev = (void *) hdev;
	skb->pkt_type = HCI_SCODATA_PKT;
	skb_queue_tail(&conn->data_q, skb);
	hci_sched_tx(hdev);

	return 0;
}

/* Handle HCI Event packets */

/* Command Complete OGF LINK_CTL  */
static void hci_cc_link_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
{
	DBG("%s ocf 0x%x", hdev->name, ocf);

	switch (ocf) {
	default:
		DBG("%s Command complete: ogf LINK_CTL ocf %x", hdev->name, ocf);
		break;
	};
}

/* Command Complete OGF LINK_POLICY  */
static void hci_cc_link_policy(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
{
	DBG("%s ocf 0x%x", hdev->name, ocf);

	switch (ocf) {
	default:
		DBG("%s: Command complete: ogf LINK_POLICY ocf %x", hdev->name, ocf);
		break;
	};
}

/* Command Complete OGF HOST_CTL  */
static void hci_cc_host_ctl(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
{
	__u8 status, param;
	void *sent;


	DBG("%s ocf 0x%x", hdev->name, ocf);