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 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 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 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 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 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