1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
|
/* SPDX-License-Identifier: GPL-2.0-or-later */
/*
*
* Generic Bluetooth HCI UART driver
*
* Copyright (C) 2015-2018 Intel Corporation
*/
#include <asm/unaligned.h>
struct h4_recv_pkt {
u8 type; /* Packet type */
u8 hlen; /* Header length */
u8 loff; /* Data length offset in header */
u8 lsize; /* Data length field size */
u16 maxlen; /* Max overall packet length */
int (*recv)(struct hci_dev *hdev, struct sk_buff *skb);
};
#define H4_RECV_ACL \
.type = HCI_ACLDATA_PKT, \
.hlen = HCI_ACL_HDR_SIZE, \
.loff = 2, \
.lsize = 2, \
.maxlen = HCI_MAX_FRAME_SIZE \
#define H4_RECV_SCO \
.type = HCI_SCODATA_PKT, \
.hlen = HCI_SCO_HDR_SIZE, \
.loff = 2, \
.lsize = 1, \
.maxlen = HCI_MAX_SCO_SIZE
#define H4_RECV_EVENT \
.type = HCI_EVENT_PKT, \
.hlen = HCI_EVENT_HDR_SIZE, \
.loff = 1, \
.lsize = 1, \
.maxlen = HCI_MAX_EVENT_SIZE
#define H4_RECV_ISO \
.type = HCI_ISODATA_PKT, \
.hlen = HCI_ISO_HDR_SIZE, \
.loff = 2, \
.lsize = 2, \
.maxlen = HCI_MAX_FRAME_SIZE
static inline struct sk_buff *h4_recv_buf(struct hci_dev *hdev,
struct sk_buff *skb,
const unsigned char *buffer,
int count,
const struct h4_recv_pkt *pkts,
int pkts_count)
{
/* Check for error from previous call */
if (IS_ERR(skb))
skb = NULL;
while (count) {
int i, len;
if (!skb) {
for (i = 0; i < pkts_count; i++) {
if (buffer[0] != (&pkts[i])->type)
continue;
skb = bt_skb_alloc((&pkts[i])->maxlen,
GFP_ATOMIC);
if (!skb)
return ERR_PTR(-ENOMEM);
hci_skb_pkt_type(skb) = (&pkts[i])->type;
hci_skb_expect(skb) = (&pkts[i])->hlen;
break;
}
/* Check for invalid packet type */
if (!skb)
return ERR_PTR(-EILSEQ);
count -= 1;
buffer += 1;
}
len = min_t(uint, hci_skb_expect(skb) - skb->len, count);
skb_put_data(skb, buffer, len);
count -= len;
buffer += len;
/* Check for partial packet */
if (skb->len < hci_skb_expect(skb))
continue;
for (i = 0; i < pkts_count; i++) {
if (hci_skb_pkt_type(skb) == (&pkts[i])->type)
break;
}
if (i >= pkts_count) {
kfree_skb(skb);
return ERR_PTR(-EILSEQ);
}
if (skb->len == (&pkts[i])->hlen) {
u16 dlen;
switch ((&pkts[i])->lsize) {
case 0:
/* No variable data length */
dlen = 0;
break;
case 1:
/* Single octet variable length */
dlen = skb->data[(&pkts[i])->loff];
hci_skb_expect(skb) += dlen;
if (skb_tailroom(skb) < dlen) {
kfree_skb(skb);
return ERR_PTR(-EMSGSIZE);
}
break;
case 2:
/* Double octet variable length */
dlen = get_unaligned_le16(skb->data +
(&pkts[i])->loff);
hci_skb_expect(skb) += dlen;
if (skb_tailroom(skb) < dlen) {
kfree_skb(skb);
return ERR_PTR(-EMSGSIZE);
}
break;
default:
/* Unsupported variable length */
kfree_skb(skb);
return ERR_PTR(-EILSEQ);
}
if (!dlen) {
/* No more data, complete frame */
(&pkts[i])->recv(hdev, skb);
skb = NULL;
}
} else {
/* Complete frame */
(&pkts[i])->recv(hdev, skb);
skb = NULL;
}
}
return skb;
}
|
