/*
|
* Licensed to the Apache Software Foundation (ASF) under one
|
* or more contributor license agreements. See the NOTICE file
|
* distributed with this work for additional information
|
* regarding copyright ownership. The ASF licenses this file
|
* to you under the Apache License, Version 2.0 (the
|
* "License"); you may not use this file except in compliance
|
* with the License. You may obtain a copy of the License at
|
*
|
* http://www.apache.org/licenses/LICENSE-2.0
|
*
|
* Unless required by applicable law or agreed to in writing,
|
* software distributed under the License is distributed on an
|
* "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY
|
* KIND, either express or implied. See the License for the
|
* specific language governing permissions and limitations
|
* under the License.
|
*/
|
|
#include <assert.h>
|
#include <stdint.h>
|
#include <string.h>
|
#include <syscfg/syscfg.h>
|
#include <os/os.h>
|
#include <os/os_mbuf.h>
|
#include <nimble/hci_common.h>
|
#include <nimble/transport.h>
|
#include <nimble/transport/hci_h4.h>
|
|
#define HCI_H4_SM_W4_PKT_TYPE 0
|
#define HCI_H4_SM_W4_HEADER 1
|
#define HCI_H4_SM_W4_PAYLOAD 2
|
#define HCI_H4_SM_COMPLETED 3
|
|
const struct hci_h4_allocators hci_h4_allocs_from_ll = {
|
.acl = ble_transport_alloc_acl_from_ll,
|
.evt = ble_transport_alloc_evt,
|
};
|
const struct hci_h4_allocators hci_h4_allocs_from_hs = {
|
.cmd = ble_transport_alloc_cmd,
|
.acl = ble_transport_alloc_acl_from_hs,
|
};
|
|
struct hci_h4_input_buffer {
|
const uint8_t *buf;
|
uint16_t len;
|
};
|
|
static void
|
hci_h4_frame_start(struct hci_h4_sm *rxs, uint8_t pkt_type)
|
{
|
rxs->pkt_type = pkt_type;
|
rxs->len = 0;
|
rxs->exp_len = 0;
|
|
switch (rxs->pkt_type) {
|
case HCI_H4_CMD:
|
rxs->min_len = 3;
|
break;
|
case HCI_H4_ACL:
|
rxs->min_len = 4;
|
break;
|
case HCI_H4_EVT:
|
rxs->min_len = 2;
|
break;
|
default:
|
/* XXX sync loss */
|
assert(0);
|
break;
|
}
|
}
|
|
static int
|
hci_h4_ib_consume(struct hci_h4_input_buffer *ib, uint16_t len)
|
{
|
assert(ib->len >= len);
|
|
ib->buf += len;
|
ib->len -= len;
|
|
return len;
|
}
|
|
static int
|
hci_h4_ib_pull_min_len(struct hci_h4_sm *rxs,
|
struct hci_h4_input_buffer *ib)
|
{
|
uint16_t len;
|
|
len = min(ib->len, rxs->min_len - rxs->len);
|
memcpy(&rxs->hdr[rxs->len], ib->buf, len);
|
|
rxs->len += len;
|
hci_h4_ib_consume(ib, len);
|
|
return rxs->len != rxs->min_len;
|
}
|
|
static int
|
hci_h4_sm_w4_header(struct hci_h4_sm *h4sm, struct hci_h4_input_buffer *ib)
|
{
|
int rc;
|
|
rc = hci_h4_ib_pull_min_len(h4sm, ib);
|
if (rc) {
|
/* need more data */
|
return 1;
|
}
|
|
switch (h4sm->pkt_type) {
|
case HCI_H4_CMD:
|
assert(h4sm->allocs && h4sm->allocs->cmd);
|
h4sm->buf = h4sm->allocs->cmd();
|
if (!h4sm->buf) {
|
return -1;
|
}
|
|
memcpy(h4sm->buf, h4sm->hdr, h4sm->len);
|
h4sm->exp_len = h4sm->hdr[2] + 3;
|
break;
|
case HCI_H4_ACL:
|
assert(h4sm->allocs && h4sm->allocs->acl);
|
h4sm->om = h4sm->allocs->acl();
|
if (!h4sm->om) {
|
return -1;
|
}
|
|
os_mbuf_append(h4sm->om, h4sm->hdr, h4sm->len);
|
h4sm->exp_len = get_le16(&h4sm->hdr[2]) + 4;
|
break;
|
case HCI_H4_EVT:
|
if (h4sm->hdr[0] == BLE_HCI_EVCODE_LE_META) {
|
/* For LE Meta event we need 3 bytes to parse header */
|
h4sm->min_len = 3;
|
rc = hci_h4_ib_pull_min_len(h4sm, ib);
|
if (rc) {
|
/* need more data */
|
return 1;
|
}
|
}
|
|
assert(h4sm->allocs && h4sm->allocs->evt);
|
|
/* We can drop legacy advertising events if there's no free buffer in
|
* discardable pool.
|
*/
|
if (h4sm->hdr[2] == BLE_HCI_LE_SUBEV_ADV_RPT) {
|
h4sm->buf = h4sm->allocs->evt(1);
|
} else {
|
h4sm->buf = h4sm->allocs->evt(0);
|
if (!h4sm->buf) {
|
return -1;
|
}
|
}
|
|
if (h4sm->buf) {
|
memcpy(h4sm->buf, h4sm->hdr, h4sm->len);
|
}
|
|
h4sm->exp_len = h4sm->hdr[1] + 2;
|
break;
|
default:
|
assert(0);
|
break;
|
}
|
|
return 0;
|
}
|
|
static int
|
hci_h4_sm_w4_payload(struct hci_h4_sm *h4sm,
|
struct hci_h4_input_buffer *ib)
|
{
|
uint16_t mbuf_len;
|
uint16_t len;
|
int rc;
|
|
len = min(ib->len, h4sm->exp_len - h4sm->len);
|
|
switch (h4sm->pkt_type) {
|
case HCI_H4_CMD:
|
case HCI_H4_EVT:
|
if (h4sm->buf) {
|
memcpy(&h4sm->buf[h4sm->len], ib->buf, len);
|
}
|
break;
|
case HCI_H4_ACL:
|
assert(h4sm->om);
|
|
mbuf_len = OS_MBUF_PKTLEN(h4sm->om);
|
rc = os_mbuf_append(h4sm->om, ib->buf, len);
|
if (rc) {
|
/* Some data may already be appended so need to adjust h4sm only by
|
* the size of appended data.
|
*/
|
len = OS_MBUF_PKTLEN(h4sm->om) - mbuf_len;
|
h4sm->len += len;
|
hci_h4_ib_consume(ib, len);
|
|
return -1;
|
}
|
break;
|
default:
|
assert(0);
|
break;
|
}
|
|
h4sm->len += len;
|
hci_h4_ib_consume(ib, len);
|
|
/* return 1 if need more data */
|
return h4sm->len != h4sm->exp_len;
|
}
|
|
static void
|
hci_h4_sm_completed(struct hci_h4_sm *h4sm)
|
{
|
int rc;
|
|
switch (h4sm->pkt_type) {
|
case HCI_H4_CMD:
|
case HCI_H4_EVT:
|
if (h4sm->buf) {
|
assert(h4sm->frame_cb);
|
rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->buf);
|
if (rc != 0) {
|
ble_transport_free(h4sm->buf);
|
}
|
h4sm->buf = NULL;
|
}
|
break;
|
case HCI_H4_ACL:
|
if (h4sm->om) {
|
assert(h4sm->frame_cb);
|
rc = h4sm->frame_cb(h4sm->pkt_type, h4sm->om);
|
if (rc != 0) {
|
os_mbuf_free_chain(h4sm->om);
|
}
|
h4sm->om = NULL;
|
}
|
break;
|
default:
|
assert(0);
|
break;
|
}
|
}
|
|
int
|
hci_h4_sm_rx(struct hci_h4_sm *h4sm, const uint8_t *buf, uint16_t len)
|
{
|
struct hci_h4_input_buffer ib = {
|
.buf = buf,
|
.len = len,
|
};
|
|
int rc = 0;
|
while (ib.len && (rc >= 0)) {
|
rc = 0;
|
switch (h4sm->state) {
|
case HCI_H4_SM_W4_PKT_TYPE:
|
hci_h4_frame_start(h4sm, ib.buf[0]);
|
hci_h4_ib_consume(&ib, 1);
|
h4sm->state = HCI_H4_SM_W4_HEADER;
|
/* no break */
|
case HCI_H4_SM_W4_HEADER:
|
rc = hci_h4_sm_w4_header(h4sm, &ib);
|
assert(rc >= 0);
|
if (rc) {
|
break;
|
}
|
h4sm->state = HCI_H4_SM_W4_PAYLOAD;
|
/* no break */
|
case HCI_H4_SM_W4_PAYLOAD:
|
rc = hci_h4_sm_w4_payload(h4sm, &ib);
|
assert(rc >= 0);
|
if (rc) {
|
break;
|
}
|
h4sm->state = HCI_H4_SM_COMPLETED;
|
/* no break */
|
case HCI_H4_SM_COMPLETED:
|
hci_h4_sm_completed(h4sm);
|
h4sm->state = HCI_H4_SM_W4_PKT_TYPE;
|
break;
|
default:
|
assert(0);
|
break;
|
}
|
}
|
|
/* Calculate consumed bytes
|
*
|
* Note: we should always consume some bytes unless there is an oom error.
|
* It's also possible that we have an oom error but already consumed some
|
* data, in such case just return success and error will be returned on next
|
* pass.
|
*/
|
len = len - ib.len;
|
if (len == 0) {
|
assert(rc < 0);
|
return -1;
|
}
|
|
return len;
|
}
|
|
void
|
hci_h4_sm_init(struct hci_h4_sm *h4sm, const struct hci_h4_allocators *allocs,
|
hci_h4_frame_cb *frame_cb)
|
{
|
memset(h4sm, 0, sizeof(*h4sm));
|
h4sm->allocs = allocs;
|
h4sm->frame_cb = frame_cb;
|
}
|
