1 /*
2 * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
3 *
4 * SPDX-License-Identifier: Apache-2.0
5 */
6 /*
7 * Licensed to the Apache Software Foundation (ASF) under one
8 * or more contributor license agreements. See the NOTICE file
9 * distributed with this work for additional information
10 * regarding copyright ownership. The ASF licenses this file
11 * to you under the Apache License, Version 2.0 (the
12 * "License"); you may not use this file except in compliance
13 * with the License. You may obtain a copy of the License at
14 *
15 * http://www.apache.org/licenses/LICENSE-2.0
16 *
17 * Unless required by applicable law or agreed to in writing,
18 * software distributed under the License is distributed on an
19 * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
20 * KIND, either express or implied. See the License for the
21 * specific language governing permissions and limitations
22 * under the License.
23 */
24
25 #include <assert.h>
26 #include <stdint.h>
27 #include <string.h>
28 #include <os/os.h>
29 #include <os/os_mbuf.h>
30 #include "common/hci_driver_h4.h"
31
32 #ifndef min
33 #define min(a, b) ((a) < (b) ? (a) : (b))
34 #endif
35
36 #ifndef max
37 #define max(a, b) ((a) > (b) ? (a) : (b))
38 #endif
39
40 #define HCI_H4_SM_W4_PKT_TYPE 0
41 #define HCI_H4_SM_W4_HEADER 1
42 #define HCI_H4_SM_W4_PAYLOAD 2
43 #define HCI_H4_SM_COMPLETED 3
44
45 struct hci_h4_input_buffer {
46 const uint8_t *buf;
47 uint16_t len;
48 };
49
50 static int
hci_h4_frame_start(struct hci_h4_sm * rxs,uint8_t pkt_type)51 hci_h4_frame_start(struct hci_h4_sm *rxs, uint8_t pkt_type)
52 {
53 rxs->pkt_type = pkt_type;
54 rxs->len = 0;
55 rxs->exp_len = 0;
56
57 switch (rxs->pkt_type) {
58 case HCI_H4_CMD:
59 rxs->min_len = 3;
60 break;
61 case HCI_H4_ACL:
62 case HCI_H4_ISO:
63 rxs->min_len = 4;
64 break;
65 case HCI_H4_EVT:
66 rxs->min_len = 2;
67 break;
68 default:
69 /* !TODO: Sync loss. Need to wait for reset. */
70 return -1;
71 }
72
73 return 0;
74 }
75
76 static int
hci_h4_ib_consume(struct hci_h4_input_buffer * ib,uint16_t len)77 hci_h4_ib_consume(struct hci_h4_input_buffer *ib, uint16_t len)
78 {
79 assert(ib->len >= len);
80
81 ib->buf += len;
82 ib->len -= len;
83
84 return len;
85 }
86
87 static int
hci_h4_ib_pull_min_len(struct hci_h4_sm * rxs,struct hci_h4_input_buffer * ib)88 hci_h4_ib_pull_min_len(struct hci_h4_sm *rxs,
89 struct hci_h4_input_buffer *ib)
90 {
91 uint16_t len;
92
93 len = min(ib->len, rxs->min_len - rxs->len);
94 memcpy(&rxs->hdr[rxs->len], ib->buf, len);
95
96 rxs->len += len;
97 hci_h4_ib_consume(ib, len);
98
99
100 return rxs->len != rxs->min_len;
101 }
102
103 static int
hci_h4_sm_w4_header(struct hci_h4_sm * h4sm,struct hci_h4_input_buffer * ib)104 hci_h4_sm_w4_header(struct hci_h4_sm *h4sm, struct hci_h4_input_buffer *ib)
105 {
106 int rc;
107
108 rc = hci_h4_ib_pull_min_len(h4sm, ib);
109 if (rc) {
110 /* need more data */
111 return 1;
112 }
113
114 switch (h4sm->pkt_type) {
115 case HCI_H4_CMD:
116 assert(h4sm->allocs && h4sm->allocs->cmd);
117 h4sm->buf = h4sm->allocs->cmd();
118 if (!h4sm->buf) {
119 return -1;
120 }
121
122 memcpy(h4sm->buf, h4sm->hdr, h4sm->len);
123 h4sm->exp_len = h4sm->hdr[2] + 3;
124
125 break;
126 case HCI_H4_ACL:
127 assert(h4sm->allocs && h4sm->allocs->acl);
128 h4sm->om = h4sm->allocs->acl();
129 if (!h4sm->om) {
130 return -1;
131 }
132
133 os_mbuf_append(h4sm->om, h4sm->hdr, h4sm->len);
134 h4sm->exp_len = get_le16(&h4sm->hdr[2]) + 4;
135 break;
136 #if !CONFIG_BT_CONTROLLER_ENABLED
137 case HCI_H4_EVT:
138 if (h4sm->hdr[0] == BLE_HCI_EVCODE_LE_META) {
139 /* For LE Meta event we need 3 bytes to parse header */
140 h4sm->min_len = 3;
141 rc = hci_h4_ib_pull_min_len(h4sm, ib);
142 if (rc) {
143 /* need more data */
144 return 1;
145 }
146 }
147
148 assert(h4sm->allocs && h4sm->allocs->evt);
149
150 /* We can drop legacy advertising events if there's no free buffer in
151 * discardable pool.
152 */
153 if (h4sm->hdr[2] == BLE_HCI_LE_SUBEV_ADV_RPT) {
154 h4sm->buf = h4sm->allocs->evt(1);
155 } else {
156 h4sm->buf = h4sm->allocs->evt(0);
157 if (!h4sm->buf) {
158 return -1;
159 }
160 }
161
162 if (h4sm->buf) {
163 memcpy(h4sm->buf, h4sm->hdr, h4sm->len);
164 }
165
166 h4sm->exp_len = h4sm->hdr[1] + 2;
167 break;
168 #endif // !CONFIG_BT_CONTROLLER_ENABLED
169 case HCI_H4_ISO:
170 assert(h4sm->allocs && h4sm->allocs->iso);
171 h4sm->om = h4sm->allocs->iso();
172 if (!h4sm->om) {
173 return -1;
174 }
175
176 os_mbuf_append(h4sm->om, h4sm->hdr, h4sm->len);
177 h4sm->exp_len = (get_le16(&h4sm->hdr[2]) & 0x7fff) + 4;
178 break;
179 default:
180 assert(0);
181 break;
182 }
183
184 return 0;
185 }
186
187 static int
hci_h4_sm_w4_payload(struct hci_h4_sm * h4sm,struct hci_h4_input_buffer * ib)188 hci_h4_sm_w4_payload(struct hci_h4_sm *h4sm,
189 struct hci_h4_input_buffer *ib)
190 {
191 uint16_t mbuf_len;
192 uint16_t len;
193 int rc;
194
195 len = min(ib->len, h4sm->exp_len - h4sm->len);
196
197
198 switch (h4sm->pkt_type) {
199 case HCI_H4_CMD:
200 case HCI_H4_EVT:
201 if (h4sm->buf) {
202 memcpy(&h4sm->buf[h4sm->len], ib->buf, len);
203 }
204 break;
205 case HCI_H4_ACL:
206 case HCI_H4_ISO:
207 assert(h4sm->om);
208
209 mbuf_len = OS_MBUF_PKTLEN(h4sm->om);
210 rc = os_mbuf_append(h4sm->om, ib->buf, len);
211 if (rc) {
212 /* Some data may already be appended so need to adjust h4sm only by
213 * the size of appended data.
214 */
215 len = OS_MBUF_PKTLEN(h4sm->om) - mbuf_len;
216 h4sm->len += len;
217 hci_h4_ib_consume(ib, len);
218
219 return -1;
220 }
221 break;
222 default:
223 assert(0);
224 break;
225 }
226
227 h4sm->len += len;
228 hci_h4_ib_consume(ib, len);
229
230 /* return 1 if need more data */
231 return h4sm->len != h4sm->exp_len;
232 }
233
234 static void
hci_h4_sm_completed(struct hci_h4_sm * h4sm)235 hci_h4_sm_completed(struct hci_h4_sm *h4sm)
236 {
237 int rc;
238
239 switch (h4sm->pkt_type) {
240 #if CONFIG_BT_CONTROLLER_ENABLED
241 case HCI_H4_CMD:
242 if (h4sm->buf) {
243 assert(h4sm->frame_cb);
244 rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->buf);
245 assert(rc == 0);
246 h4sm->buf = NULL;
247 }
248 break;
249 case HCI_H4_ACL:
250 case HCI_H4_ISO:
251 if (h4sm->om) {
252 assert(h4sm->frame_cb);
253 rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->om);
254 assert(rc == 0);
255 h4sm->om = NULL;
256 }
257 break;
258 #else
259 case HCI_H4_CMD:
260 case HCI_H4_EVT:
261 if (h4sm->buf) {
262 assert(h4sm->frame_cb);
263 rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->buf);
264 if (rc != 0) {
265 ble_transport_free(h4sm->buf);
266 }
267 h4sm->buf = NULL;
268 }
269 break;
270 case HCI_H4_ACL:
271 case HCI_H4_ISO:
272 if (h4sm->om) {
273 assert(h4sm->frame_cb);
274 rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->om);
275 if (rc != 0) {
276 os_mbuf_free_chain(h4sm->om);
277 }
278 h4sm->om = NULL;
279 }
280 break;
281 #endif // CONFIG_BT_CONTROLLER_ENABLED
282 default:
283 assert(0);
284 break;
285 }
286 }
287
288 int
hci_h4_sm_rx(struct hci_h4_sm * h4sm,const uint8_t * buf,uint16_t len)289 hci_h4_sm_rx(struct hci_h4_sm *h4sm, const uint8_t *buf, uint16_t len)
290 {
291 struct hci_h4_input_buffer ib = {
292 .buf = buf,
293 .len = len,
294 };
295
296 int rc = 0;
297 while (ib.len && (rc >= 0)) {
298 rc = 0;
299 switch (h4sm->state) {
300 case HCI_H4_SM_W4_PKT_TYPE:
301 if (hci_h4_frame_start(h4sm, ib.buf[0]) < 0) {
302 return -1;
303 }
304
305 hci_h4_ib_consume(&ib, 1);
306 h4sm->state = HCI_H4_SM_W4_HEADER;
307 /* no break */
308 case HCI_H4_SM_W4_HEADER:
309 rc = hci_h4_sm_w4_header(h4sm, &ib);
310 assert(rc >= 0);
311 if (rc) {
312 break;
313 }
314 h4sm->state = HCI_H4_SM_W4_PAYLOAD;
315 /* no break */
316 case HCI_H4_SM_W4_PAYLOAD:
317 rc = hci_h4_sm_w4_payload(h4sm, &ib);
318 assert(rc >= 0);
319 if (rc) {
320 break;
321 }
322 h4sm->state = HCI_H4_SM_COMPLETED;
323 /* no break */
324 case HCI_H4_SM_COMPLETED:
325 hci_h4_sm_completed(h4sm);
326 h4sm->state = HCI_H4_SM_W4_PKT_TYPE;
327 break;
328 default:
329 return -1;
330 }
331 }
332
333 /* Calculate consumed bytes
334 *
335 * Note: we should always consume some bytes unless there is an oom error.
336 * It's also possible that we have an oom error but already consumed some
337 * data, in such case just return success and error will be returned on next
338 * pass.
339 */
340 len = len - ib.len;
341 if (len == 0) {
342 assert(rc < 0);
343 return -1;
344 }
345
346 return len;
347 }
348
349 void
hci_h4_sm_init(struct hci_h4_sm * h4sm,const struct hci_h4_allocators * allocs,hci_h4_frame_cb * frame_cb)350 hci_h4_sm_init(struct hci_h4_sm *h4sm, const struct hci_h4_allocators *allocs,
351 hci_h4_frame_cb *frame_cb)
352 {
353 memset(h4sm, 0, sizeof(*h4sm));
354 h4sm->allocs = allocs;
355 h4sm->frame_cb = frame_cb;
356 }
357
