net/bluetooth/msft.c

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