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
154
155
156
157
158
159
160
|
// SPDX-License-Identifier: GPL-2.0-or-later
/*
* Bluetooth HCI H:4 packet reassembly
*
* Copyright (C) 2000-2001 Qualcomm Incorporated
* Copyright (C) 2002-2003 Maxim Krasnyansky <maxk@qualcomm.com>
* Copyright (C) 2004-2005 Marcel Holtmann <marcel@holtmann.org>
*/
#include <linux/export.h>
#include <linux/skbuff.h>
#include <linux/unaligned.h>
#include <net/bluetooth/bluetooth.h>
#include <net/bluetooth/hci_core.h>
#include <net/bluetooth/hci_h4.h>
/* h4_recv_skb - Reassemble H:4 framed packets
* @hdev: HCI device the packets are received on
* @alignment: optional packet alignment, NULL or 0 means no alignment
* @padding: optional padding state carried over between calls
* @skb: partially received packet from a previous call, may be NULL or an
* ERR_PTR returned by a previous call
* @buffer: buffer holding the received data
* @count: number of bytes in @buffer
* @pkts: table of supported packet types
* @pkts_count: number of entries in @pkts
*
* Returns the partially received packet to be passed to the next call, or an
* ERR_PTR on error. The returned value can be fed back into this function as
* is, but must be checked with IS_ERR() before being freed.
*/
struct sk_buff *h4_recv_skb(struct hci_dev *hdev, u8 *alignment, u8 *padding,
struct sk_buff *skb, const unsigned char *buffer,
int count, const struct h4_recv_pkt *pkts,
int pkts_count)
{
u8 align = alignment && *alignment ? *alignment : 1;
/* Check for error from previous call */
if (IS_ERR(skb))
skb = NULL;
while (count) {
int i, len;
/* remove padding bytes from buffer */
if (padding) {
for (; (*padding) && count > 0; (*padding)--) {
count--;
buffer++;
}
}
if (!count)
break;
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) {
if (padding) {
*padding = (skb->len + 1) % align;
*padding = (align - *padding) % align;
}
/* No more data, complete frame */
pkts[i].recv(hdev, skb);
skb = NULL;
}
} else {
if (padding) {
*padding = (skb->len + 1) % align;
*padding = (align - *padding) % align;
}
/* Complete frame */
pkts[i].recv(hdev, skb);
skb = NULL;
}
}
return skb;
}
EXPORT_SYMBOL_GPL(h4_recv_skb);
|
