hci_core.c

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

	switch (ocf) {
	case OCF_RESET:
		status = *((__u8 *) skb->data);

		hci_req_complete(hdev, status);
		break;

	case OCF_SET_EVENT_FLT:
		status = *((__u8 *) skb->data);

		if (status) {
			DBG("%s SET_EVENT_FLT failed %d", hdev->name, status);
		} else {
			DBG("%s SET_EVENT_FLT succeseful", hdev->name);
		}
		break;

	case OCF_WRITE_AUTH_ENABLE:
		if (!(sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_AUTH_ENABLE)))
			break;

		status = *((__u8 *) skb->data);
		param  = *((__u8 *) sent);

		if (!status) {
			if (param == AUTH_ENABLED)
				hdev->flags |= HCI_AUTH;
			else
				hdev->flags &= ~HCI_AUTH;
		}
		hci_req_complete(hdev, status);
		break;

	case OCF_WRITE_CA_TIMEOUT:
		status = *((__u8 *) skb->data);

		if (status) {
			DBG("%s OCF_WRITE_CA_TIMEOUT failed %d", hdev->name, status);
		} else {
			DBG("%s OCF_WRITE_CA_TIMEOUT succeseful", hdev->name);
		}
		break;

	case OCF_WRITE_PG_TIMEOUT:
		status = *((__u8 *) skb->data);

		if (status) {
			DBG("%s OCF_WRITE_PG_TIMEOUT failed %d", hdev->name, status);
		} else {
			DBG("%s: OCF_WRITE_PG_TIMEOUT succeseful", hdev->name);
		}
		break;

	case OCF_WRITE_SCAN_ENABLE:
		if (!(sent = hci_sent_cmd_data(hdev, OGF_HOST_CTL, OCF_WRITE_SCAN_ENABLE)))
			break;
		status = *((__u8 *) skb->data);
		param  = *((__u8 *) sent);

		DBG("param 0x%x", param);

		if (!status) {
			switch (param) {
			case IS_ENA_PS_ENA:
				hdev->flags |=  HCI_PSCAN | HCI_ISCAN;
				break;

			case IS_ENA_PS_DIS:
				hdev->flags &= ~HCI_PSCAN;
				hdev->flags |=  HCI_ISCAN;
				break;

			case IS_DIS_PS_ENA:
				hdev->flags &= ~HCI_ISCAN;
				hdev->flags |=  HCI_PSCAN;
				break;

			default:
				hdev->flags &= ~(HCI_ISCAN | HCI_PSCAN);
				break;
			};
		}
		hci_req_complete(hdev, status);
		break;

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

/* Command Complete OGF INFO_PARAM  */
static void hci_cc_info_param(struct hci_dev *hdev, __u16 ocf, struct sk_buff *skb)
{
	read_local_features_rp *lf;
	read_buffer_size_rp *bs;
	read_bd_addr_rp *ba;

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

	switch (ocf) {
	case OCF_READ_LOCAL_FEATURES:
		lf = (read_local_features_rp *) skb->data;

		if (lf->status) {
			DBG("%s READ_LOCAL_FEATURES failed %d", hdev->name, lf->status);
			break;
		}

		memcpy(hdev->features, lf->features, sizeof(hdev->features));

		/* Adjust default settings according to features 
		 * supported by device. */
		if (hdev->features[0] & LMP_3SLOT)
			hdev->pkt_type |= (HCI_DM3 | HCI_DH3);

		if (hdev->features[0] & LMP_5SLOT)
			hdev->pkt_type |= (HCI_DM5 | HCI_DH5);

		DBG("%s: features 0x%x 0x%x 0x%x", hdev->name, lf->features[0], lf->features[1], lf->features[2]);

		break;

	case OCF_READ_BUFFER_SIZE:
		bs = (read_buffer_size_rp *) skb->data;

		if (bs->status) {
			DBG("%s READ_BUFFER_SIZE failed %d", hdev->name, bs->status);
			break;
		}

		hdev->acl_mtu = __le16_to_cpu(bs->acl_mtu);
		hdev->sco_mtu = bs->sco_mtu;
		hdev->acl_max = hdev->acl_cnt = __le16_to_cpu(bs->acl_max_pkt);
		hdev->sco_max = hdev->sco_cnt = __le16_to_cpu(bs->sco_max_pkt);

		DBG("%s mtu: acl %d, sco %d max_pkt: acl %d, sco %d", hdev->name,
		    hdev->acl_mtu, hdev->sco_mtu, hdev->acl_max, hdev->sco_max);

		break;

	case OCF_READ_BD_ADDR:
		ba = (read_bd_addr_rp *) skb->data;

		if (!ba->status) {
			bacpy(&hdev->bdaddr, &ba->bdaddr);
		} else {
			DBG("%s: READ_BD_ADDR failed %d", hdev->name, ba->status);
		}

		hci_req_complete(hdev, ba->status);
		break;

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

/* Command Status OGF LINK_CTL  */
static void hci_cs_link_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
{
	struct hci_proto * hp;

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

	switch (ocf) {
	case OCF_CREATE_CONN:
		if (status) {
			create_conn_cp *cc = hci_sent_cmd_data(hdev, OGF_LINK_CTL, OCF_CREATE_CONN);

			if (!cc)
				break;

			DBG("%s Create connection error: status 0x%x %s", hdev->name,
			    status, batostr(&cc->bdaddr));

			/* Notify upper protocols */
			if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->connect_cfm) {
				tasklet_disable(&hdev->tx_task);
				hp->connect_cfm(hdev, &cc->bdaddr, status, NULL);
				tasklet_enable(&hdev->tx_task);
			}
		}
		break;

	case OCF_INQUIRY:
		if (status) {
			DBG("%s Inquiry error: status 0x%x", hdev->name, status);
			hci_req_complete(hdev, status);
		}
		break;

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

/* Command Status OGF LINK_POLICY */
static void hci_cs_link_policy(struct hci_dev *hdev, __u16 ocf, __u8 status)
{
	DBG("%s ocf 0x%x", hdev->name, ocf);

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

/* Command Status OGF HOST_CTL */
static void hci_cs_host_ctl(struct hci_dev *hdev, __u16 ocf, __u8 status)
{
	DBG("%s ocf 0x%x", hdev->name, ocf);

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

/* Command Status OGF INFO_PARAM  */
static void hci_cs_info_param(struct hci_dev *hdev, __u16 ocf, __u8 status)
{
	DBG("%s: hci_cs_info_param: ocf 0x%x", hdev->name, ocf);

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

/* Inquiry Complete */
static void hci_inquiry_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	__u8 status = *((__u8 *) skb->data);

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

	hci_req_complete(hdev, status);
}

/* Inquiry Result */
static void hci_inquiry_result_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	inquiry_info *info = (inquiry_info *) (skb->data + 1);
	int num_rsp = *((__u8 *) skb->data);

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

	for (; num_rsp; num_rsp--)
		inquiry_cache_update(&hdev->inq_cache, info++);
}

/* Connect Request */
static void hci_conn_request_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	evt_conn_request *cr = (evt_conn_request *) skb->data;
	struct hci_proto *hp;
	accept_conn_req_cp ac;
	int accept = 0;

	DBG("%s Connection request: %s type 0x%x", hdev->name, batostr(&cr->bdaddr), cr->link_type);

	/* Notify upper protocols */
	if (cr->link_type == ACL_LINK) {
		/* ACL link notify L2CAP */
		if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->connect_ind) {
			tasklet_disable(&hdev->tx_task);
			accept = hp->connect_ind(hdev, &cr->bdaddr);
			tasklet_enable(&hdev->tx_task);
		}
	} else {
		/* SCO link (no notification) */
		/* FIXME: Should be accept it here or let the requester (app) accept it ? */
		accept = 1;
	}

	if (accept) {
		/* Connection accepted by upper layer */
		bacpy(&ac.bdaddr, &cr->bdaddr);
		ac.role = 0x01; /* Remain slave */
		hci_send_cmd(hdev, OGF_LINK_CTL, OCF_ACCEPT_CONN_REQ, ACCEPT_CONN_REQ_CP_SIZE, &ac);
	} else {
		/* Connection rejected by upper layer */
		/* FIXME: 
		 * Should we use HCI reject here ?
		 */
		return;
	}
}

/* Connect Complete */
static void hci_conn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	evt_conn_complete *cc = (evt_conn_complete *) skb->data;
	struct hci_conn *conn = NULL;
	struct hci_proto *hp;

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

	tasklet_disable(&hdev->tx_task);

	if (!cc->status)
 		conn = hci_conn_add(hdev, __le16_to_cpu(cc->handle), cc->link_type, &cc->bdaddr);

	/* Notify upper protocols */
	if (cc->link_type == ACL_LINK) {
		/* ACL link notify L2CAP layer */
		if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->connect_cfm)
			hp->connect_cfm(hdev, &cc->bdaddr, cc->status, conn);
	} else {
		/* SCO link (no notification) */
	}

	tasklet_enable(&hdev->tx_task);
}

/* Disconnect Complete */
static void hci_disconn_complete_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	evt_disconn_complete *dc = (evt_disconn_complete *) skb->data;
	struct hci_conn *conn = NULL;
	struct hci_proto *hp;
	__u16 handle = __le16_to_cpu(dc->handle);

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

	if (!dc->status && (conn = conn_hash_lookup(&hdev->conn_hash, handle))) {
		tasklet_disable(&hdev->tx_task);

		/* Notify upper protocols */
		if (conn->type == ACL_LINK) {
			/* ACL link notify L2CAP layer */
			if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->disconn_ind)
				hp->disconn_ind(conn, dc->reason);
		} else {
			/* SCO link (no notification) */
		}

		hci_conn_del(hdev, conn);

		tasklet_enable(&hdev->tx_task);
	}
}

/* Number of completed packets */
static void hci_num_comp_pkts_evt(struct hci_dev *hdev, struct sk_buff *skb)
{
	evt_num_comp_pkts *nc = (evt_num_comp_pkts *) skb->data;
	__u16 *ptr;
	int i;

	skb_pull(skb, EVT_NUM_COMP_PKTS_SIZE);

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

	if (skb->len < nc->num_hndl * 4) {
		DBG("%s bad parameters", hdev->name);
		return;
	}

	tasklet_disable(&hdev->tx_task);

	for (i = 0, ptr = (__u16 *) skb->data; i < nc->num_hndl; i++) {
		struct hci_conn *conn;
		__u16 handle, count;

		handle = __le16_to_cpu(get_unaligned(ptr++));
		count  = __le16_to_cpu(get_unaligned(ptr++));

		hdev->acl_cnt += count;

		if ((conn = conn_hash_lookup(&hdev->conn_hash, handle)))
			conn->sent -= count;
	}

	tasklet_enable(&hdev->tx_task);
	
	hci_sched_tx(hdev);
}

static inline void hci_event_packet(struct hci_dev *hdev, struct sk_buff *skb)
{
	hci_event_hdr *he = (hci_event_hdr *) skb->data;
	evt_cmd_status *cs;
	evt_cmd_complete *ec;
	__u16 opcode, ocf, ogf;

	skb_pull(skb, HCI_EVENT_HDR_SIZE);

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

	switch (he->evt) {
	case EVT_NUM_COMP_PKTS:
		hci_num_comp_pkts_evt(hdev, skb);
		break;

	case EVT_INQUIRY_COMPLETE:
		hci_inquiry_complete_evt(hdev, skb);
		break;

	case EVT_INQUIRY_RESULT:
		hci_inquiry_result_evt(hdev, skb);
		break;

	case EVT_CONN_REQUEST:
		hci_conn_request_evt(hdev, skb);
		break;

	case EVT_CONN_COMPLETE:
		hci_conn_complete_evt(hdev, skb);
		break;

	case EVT_DISCONN_COMPLETE:
		hci_disconn_complete_evt(hdev, skb);
		break;

	case EVT_CMD_STATUS:
		cs = (evt_cmd_status *) skb->data;
		skb_pull(skb, EVT_CMD_STATUS_SIZE);
				
		opcode = __le16_to_cpu(cs->opcode);
		ogf = cmd_opcode_ogf(opcode);
		ocf = cmd_opcode_ocf(opcode);

		switch (ogf) {
		case OGF_INFO_PARAM:
			hci_cs_info_param(hdev, ocf, cs->status);
			break;

		case OGF_HOST_CTL:
			hci_cs_host_ctl(hdev, ocf, cs->status);
			break;

		case OGF_LINK_CTL:
			hci_cs_link_ctl(hdev, ocf, cs->status);
			break;

		case OGF_LINK_POLICY:
			hci_cs_link_policy(hdev, ocf, cs->status);
			break;

		default:
			DBG("%s Command Status OGF %x", hdev->name, ogf);
			break;
		};

		if (cs->ncmd) {
			atomic_set(&hdev->cmd_cnt, 1);
			if (!skb_queue_empty(&hdev->cmd_q))
				hci_sched_cmd(hdev);
		}
		break;

	case EVT_CMD_COMPLETE:
		ec = (evt_cmd_complete *) skb->data;
		skb_pull(skb, EVT_CMD_COMPLETE_SIZE);

		opcode = __le16_to_cpu(ec->opcode);
		ogf = cmd_opcode_ogf(opcode);
		ocf = cmd_opcode_ocf(opcode);

		switch (ogf) {
		case OGF_INFO_PARAM:
			hci_cc_info_param(hdev, ocf, skb);
			break;

		case OGF_HOST_CTL:
			hci_cc_host_ctl(hdev, ocf, skb);
			break;

		case OGF_LINK_CTL:
			hci_cc_link_ctl(hdev, ocf, skb);
			break;

		case OGF_LINK_POLICY:
			hci_cc_link_policy(hdev, ocf, skb);
			break;

		default:
			DBG("%s Command Completed OGF %x", hdev->name, ogf);
			break;
		};

		if (ec->ncmd) {
			atomic_set(&hdev->cmd_cnt, 1);
			if (!skb_queue_empty(&hdev->cmd_q))
				hci_sched_cmd(hdev);
		}
		break;
	};

	kfree_skb(skb);
	hdev->stat.evt_rx++;
}

/* ACL data packet */
static inline void hci_acldata_packet(struct hci_dev *hdev, struct sk_buff *skb)
{
	hci_acl_hdr *ah = (void *) skb->data;
	struct hci_conn *conn;
	__u16 handle, flags;

	skb_pull(skb, HCI_ACL_HDR_SIZE);

	handle = __le16_to_cpu(ah->handle);
	flags  = acl_flags(handle);
	handle = acl_handle(handle);

	DBG("%s len %d handle 0x%x flags 0x%x", hdev->name, skb->len, handle, flags);

	if ((conn = conn_hash_lookup(&hdev->conn_hash, handle))) {
		register struct hci_proto *hp;

		/* Send to upper protocol */
		if ((hp = GET_HPROTO(HCI_PROTO_L2CAP)) && hp->recv_acldata) {
			hp->recv_acldata(conn, skb, flags);
			goto sent;
		}
	} else {
		ERR("%s ACL packet for unknown connection handle %d", hdev->name, handle);
	}

	kfree_skb(skb);
sent:
	hdev->stat.acl_rx++;
}

/* SCO data packet */
static inline void hci_scodata_packet(struct hci_dev *hdev, struct sk_buff *skb)
{
	DBG("%s len %d", hdev->name, skb->len);

	kfree_skb(skb);
	hdev->stat.sco_rx++;
}

/* ----- HCI tasks ----- */
void hci_rx_task(unsigned long arg)
{
	struct hci_dev *hdev = (struct hci_dev *) arg;
	struct sk_buff *skb;

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

	read_lock(&hci_task_lock);

	while ((skb = skb_dequeue(&hdev->rx_q))) {
		if (hdev->flags & HCI_SOCK) {
			/* Send copy to the sockets */
			hci_send_to_sock(hdev, skb);
		}

		if (hdev->flags & HCI_INIT) {
			/* Don't process data packets in this states. */
			switch (skb->pkt_type) {
			case HCI_ACLDATA_PKT:
			case HCI_SCODATA_PKT:
				kfree_skb(skb);
				continue;
			};
		}

		if (hdev->flags & HCI_NORMAL) {
			/* Process frame */
			switch (skb->pkt_type) {
			case HCI_EVENT_PKT:
				hci_event_packet(hdev, skb);
				break;

			case HCI_ACLDATA_PKT:
				DBG("%s ACL data packet", hdev->name);
				hci_acldata_packet(hdev, skb);
				break;

			case HCI_SCODATA_PKT:
				DBG("%s SCO data packet", hdev->name);
				hci_scodata_packet(hdev, skb);
				break;

			default:
				kfree_skb(skb);
				break;
			};
		} else {
			kfree_skb(skb);
		}
	}

	read_unlock(&hci_task_lock);
}

static void hci_tx_task(unsigned long arg)
{
	struct hci_dev *hdev = (struct hci_dev *) arg;
	struct sk_buff *skb;

	read_lock(&hci_task_lock);

	DBG("%s acl %d sco %d", hdev->name, hdev->acl_cnt, hdev->sco_cnt);

	/* Schedule queues and send stuff to HCI driver */

	hci_sched_acl(hdev);

	hci_sched_sco(hdev);

	/* Send next queued raw (unknown type) packet */
	while ((skb = skb_dequeue(&hdev->raw_q)))
		hci_send_frame(skb);

	read_unlock(&hci_task_lock);
}

static void hci_cmd_task(unsigned long arg)
{
	struct hci_dev *hdev = (struct hci_dev *) arg;
	struct sk_buff *skb;

	DBG("%s cmd %d", hdev->name, atomic_read(&hdev->cmd_cnt));

	/* Send queued commands */
	if (atomic_read(&hdev->cmd_cnt) && (skb = skb_dequeue(&hdev->cmd_q))) {
		if (hdev->sent_cmd)
			kfree_skb(hdev->sent_cmd);

		if ((hdev->sent_cmd = skb_clone(skb, GFP_ATOMIC))) {
			atomic_dec(&hdev->cmd_cnt);
			hci_send_frame(skb);
		} else {
			skb_queue_head(&hdev->cmd_q, skb);
			hci_sched_cmd(hdev);
		}
	}
}

/* Receive frame from HCI drivers */
int hci_recv_frame(struct sk_buff *skb)
{
	struct hci_dev *hdev = (struct hci_dev *) skb->dev;

	if (!hdev || !(hdev->flags & (HCI_UP | HCI_INIT))) {
		kfree_skb(skb);
		return -1;
	}

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

	/* Incomming skb */
	bluez_cb(skb)->incomming = 1;

	/* Queue frame for rx task */
	skb_queue_tail(&hdev->rx_q, skb);
	hci_sched_rx(hdev);

	return 0;
}

int hci_core_init(void)
{
	/* Init locks */
	spin_lock_init(&hdev_list_lock);

	return 0;
}

int hci_core_cleanup(void)
{
	return 0;
}

MODULE_LICENSE("GPL");