1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * Copyright (C) 2020 Google Corporation
4 */
5
6 #include <net/bluetooth/bluetooth.h>
7 #include <net/bluetooth/hci_core.h>
8
9 #include "msft.h"
10
11 #define MSFT_OP_READ_SUPPORTED_FEATURES 0x00
12 struct msft_cp_read_supported_features {
13 __u8 sub_opcode;
14 } __packed;
15
16 struct msft_rp_read_supported_features {
17 __u8 status;
18 __u8 sub_opcode;
19 __le64 features;
20 __u8 evt_prefix_len;
21 __u8 evt_prefix[];
22 } __packed;
23
24 struct msft_data {
25 __u64 features;
26 __u8 evt_prefix_len;
27 __u8 *evt_prefix;
28 };
29
read_supported_features(struct hci_dev * hdev,struct msft_data * msft)30 static bool read_supported_features(struct hci_dev *hdev,
31 struct msft_data *msft)
32 {
33 struct msft_cp_read_supported_features cp;
34 struct msft_rp_read_supported_features *rp;
35 struct sk_buff *skb;
36
37 cp.sub_opcode = MSFT_OP_READ_SUPPORTED_FEATURES;
38
39 skb = __hci_cmd_sync(hdev, hdev->msft_opcode, sizeof(cp), &cp,
40 HCI_CMD_TIMEOUT);
41 if (IS_ERR(skb)) {
42 bt_dev_err(hdev, "Failed to read MSFT supported features (%ld)",
43 PTR_ERR(skb));
44 return false;
45 }
46
47 if (skb->len < sizeof(*rp)) {
48 bt_dev_err(hdev, "MSFT supported features length mismatch");
49 goto failed;
50 }
51
52 rp = (struct msft_rp_read_supported_features *)skb->data;
53
54 if (rp->sub_opcode != MSFT_OP_READ_SUPPORTED_FEATURES)
55 goto failed;
56
57 if (rp->evt_prefix_len > 0) {
58 msft->evt_prefix = kmemdup(rp->evt_prefix, rp->evt_prefix_len,
59 GFP_KERNEL);
60 if (!msft->evt_prefix)
61 goto failed;
62 }
63
64 msft->evt_prefix_len = rp->evt_prefix_len;
65 msft->features = __le64_to_cpu(rp->features);
66
67 kfree_skb(skb);
68 return true;
69
70 failed:
71 kfree_skb(skb);
72 return false;
73 }
74
msft_do_open(struct hci_dev * hdev)75 void msft_do_open(struct hci_dev *hdev)
76 {
77 struct msft_data *msft;
78
79 if (hdev->msft_opcode == HCI_OP_NOP)
80 return;
81
82 bt_dev_dbg(hdev, "Initialize MSFT extension");
83
84 msft = kzalloc(sizeof(*msft), GFP_KERNEL);
85 if (!msft)
86 return;
87
88 if (!read_supported_features(hdev, msft)) {
89 kfree(msft);
90 return;
91 }
92
93 hdev->msft_data = msft;
94 }
95
msft_do_close(struct hci_dev * hdev)96 void msft_do_close(struct hci_dev *hdev)
97 {
98 struct msft_data *msft = hdev->msft_data;
99
100 if (!msft)
101 return;
102
103 bt_dev_dbg(hdev, "Cleanup of MSFT extension");
104
105 hdev->msft_data = NULL;
106
107 kfree(msft->evt_prefix);
108 kfree(msft);
109 }
110
msft_vendor_evt(struct hci_dev * hdev,struct sk_buff * skb)111 void msft_vendor_evt(struct hci_dev *hdev, struct sk_buff *skb)
112 {
113 struct msft_data *msft = hdev->msft_data;
114 u8 event;
115
116 if (!msft)
117 return;
118
119 /* When the extension has defined an event prefix, check that it
120 * matches, and otherwise just return.
121 */
122 if (msft->evt_prefix_len > 0) {
123 if (skb->len < msft->evt_prefix_len)
124 return;
125
126 if (memcmp(skb->data, msft->evt_prefix, msft->evt_prefix_len))
127 return;
128
129 skb_pull(skb, msft->evt_prefix_len);
130 }
131
132 /* Every event starts at least with an event code and the rest of
133 * the data is variable and depends on the event code.
134 */
135 if (skb->len < 1)
136 return;
137
138 event = *skb->data;
139 skb_pull(skb, 1);
140
141 bt_dev_dbg(hdev, "MSFT vendor event %u", event);
142 }
143
msft_get_features(struct hci_dev * hdev)144 __u64 msft_get_features(struct hci_dev *hdev)
145 {
146 struct msft_data *msft = hdev->msft_data;
147
148 return msft ? msft->features : 0;
149 }
150
