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