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