msft.c 2.9 KB

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