Bluetooth: A2MP: Process A2MP Discover Request (8598d064) · Commits · 戴 / test

include/net/bluetooth/a2mp.h

+2 −0

Original line number	Diff line number	Diff line
		@@ -15,6 +15,8 @@
		#ifndef __A2MP_H
		#define __A2MP_H

		#define A2MP_FEAT_EXT 0x8000

		struct amp_mgr {
		struct l2cap_conn *l2cap_conn;
		struct l2cap_chan *a2mp_chan;

include/net/bluetooth/hci.h

+3 −0

Original line number	Diff line number	Diff line
		@@ -59,6 +59,9 @@
		#define HCI_BREDR 0x00
		#define HCI_AMP 0x01

		/* First BR/EDR Controller shall have ID = 0 */
		#define HCI_BREDR_ID 0

		/* HCI device quirks */
		enum {
		HCI_QUIRK_RESET_ON_CLOSE,

include/net/bluetooth/hci_core.h

+13 −0

Original line number	Diff line number	Diff line
		@@ -641,6 +641,19 @@ static inline void hci_set_drvdata(struct hci_dev hdev, void data)
		dev_set_drvdata(&hdev->dev, data);
		}

		/* hci_dev_list shall be locked */
		static inline uint8_t __hci_num_ctrl(void)
		{
		uint8_t count = 0;
		struct list_head *p;

		list_for_each(p, &hci_dev_list) {
		count++;
		}

		return count;
		}

		struct hci_dev *hci_dev_get(int index);
		struct hci_dev hci_get_route(bdaddr_t src, bdaddr_t *dst);

net/bluetooth/a2mp.c

+85 −0

Original line number	Diff line number	Diff line
		@@ -63,6 +63,36 @@ static void a2mp_send(struct amp_mgr *mgr, u8 code, u8 ident, u16 len,
		kfree(cmd);
		}

		static inline void __a2mp_cl_bredr(struct a2mp_cl *cl)
		{
		cl->id = 0;
		cl->type = 0;
		cl->status = 1;
		}

		/* hci_dev_list shall be locked */
		static void __a2mp_add_cl(struct amp_mgr mgr, struct a2mp_cl cl, u8 num_ctrl)
		{
		int i = 0;
		struct hci_dev *hdev;

		__a2mp_cl_bredr(cl);

		list_for_each_entry(hdev, &hci_dev_list, list) {
		/* Iterate through AMP controllers */
		if (hdev->id == HCI_BREDR_ID)
		continue;

		/* Starting from second entry */
		if (++i >= num_ctrl)
		return;

		cl[i].id = hdev->id;
		cl[i].type = hdev->amp_type;
		cl[i].status = hdev->amp_status;
		}
		}

		/* Processing A2MP messages */
		static int a2mp_command_rej(struct amp_mgr mgr, struct sk_buff skb,
		struct a2mp_cmd *hdr)
		@@ -79,6 +109,58 @@ static int a2mp_command_rej(struct amp_mgr mgr, struct sk_buff skb,
		return 0;
		}

		static int a2mp_discover_req(struct amp_mgr mgr, struct sk_buff skb,
		struct a2mp_cmd *hdr)
		{
		struct a2mp_discov_req req = (void ) skb->data;
		u16 len = le16_to_cpu(hdr->len);
		struct a2mp_discov_rsp *rsp;
		u16 ext_feat;
		u8 num_ctrl;

		if (len < sizeof(*req))
		return -EINVAL;

		skb_pull(skb, sizeof(*req));

		ext_feat = le16_to_cpu(req->ext_feat);

		BT_DBG("mtu %d efm 0x%4.4x", le16_to_cpu(req->mtu), ext_feat);

		/* check that packet is not broken for now */
		while (ext_feat & A2MP_FEAT_EXT) {
		if (len < sizeof(ext_feat))
		return -EINVAL;

		ext_feat = get_unaligned_le16(skb->data);
		BT_DBG("efm 0x%4.4x", ext_feat);
		len -= sizeof(ext_feat);
		skb_pull(skb, sizeof(ext_feat));
		}

		read_lock(&hci_dev_list_lock);

		num_ctrl = __hci_num_ctrl();
		len = num_ctrl * sizeof(struct a2mp_cl) + sizeof(*rsp);
		rsp = kmalloc(len, GFP_ATOMIC);
		if (!rsp) {
		read_unlock(&hci_dev_list_lock);
		return -ENOMEM;
		}

		rsp->mtu = __constant_cpu_to_le16(L2CAP_A2MP_DEFAULT_MTU);
		rsp->ext_feat = 0;

		__a2mp_add_cl(mgr, rsp->cl, num_ctrl);

		read_unlock(&hci_dev_list_lock);

		a2mp_send(mgr, A2MP_DISCOVER_RSP, hdr->ident, len, rsp);

		kfree(rsp);
		return 0;
		}

		/* Handle A2MP signalling */
		static int a2mp_chan_recv_cb(struct l2cap_chan chan, struct sk_buff skb)
		{
		@@ -109,6 +191,9 @@ static int a2mp_chan_recv_cb(struct l2cap_chan chan, struct sk_buff skb)
		break;

		case A2MP_DISCOVER_REQ:
		err = a2mp_discover_req(mgr, skb, hdr);
		break;

		case A2MP_CHANGE_NOTIFY:
		case A2MP_GETINFO_REQ:
		case A2MP_GETAMPASSOC_REQ:

Admin message