/*
|
* 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.
|
*/
|
|
/* l2cap.c - Bluetooth L2CAP Tester */
|
|
/*
|
* Copyright (c) 2016 Intel Corporation
|
*
|
* SPDX-License-Identifier: Apache-2.0
|
*/
|
|
#include "nimble_syscfg.h"
|
|
#if MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
|
|
#include "console/console.h"
|
#include "host/ble_gap.h"
|
#include "host/ble_l2cap.h"
|
|
#include "../../../nimble/host/src/ble_l2cap_priv.h"
|
|
#include "bttester.h"
|
|
#define CONTROLLER_INDEX 0
|
#define CHANNELS MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM)
|
#define TESTER_COC_MTU MYNEWT_VAL(BTTESTER_L2CAP_COC_MTU)
|
#define TESTER_COC_BUF_COUNT (3 * MYNEWT_VAL(BLE_L2CAP_COC_MAX_NUM))
|
|
static os_membuf_t tester_sdu_coc_mem[
|
OS_MEMPOOL_SIZE(TESTER_COC_BUF_COUNT, TESTER_COC_MTU)
|
];
|
|
struct os_mbuf_pool sdu_os_mbuf_pool;
|
static struct os_mempool sdu_coc_mbuf_mempool;
|
static bool hold_credit = false;
|
|
static struct channel {
|
uint8_t chan_id; /* Internal number that identifies L2CAP channel. */
|
uint8_t state;
|
struct ble_l2cap_chan *chan;
|
} channels[CHANNELS];
|
|
static uint8_t recv_cb_buf[TESTER_COC_MTU + sizeof(struct l2cap_data_received_ev)];
|
|
static struct channel *get_free_channel(void)
|
{
|
uint8_t i;
|
struct channel *chan;
|
|
for (i = 0; i < CHANNELS; i++) {
|
if (channels[i].state) {
|
continue;
|
}
|
|
chan = &channels[i];
|
chan->chan_id = i;
|
|
return chan;
|
}
|
|
return NULL;
|
}
|
|
struct channel *find_channel(struct ble_l2cap_chan *chan)
|
{
|
int i;
|
|
for (i = 0; i < CHANNELS; ++i) {
|
if (channels[i].chan == chan) {
|
return &channels[i];
|
}
|
}
|
|
return NULL;
|
}
|
|
struct channel *get_channel(uint8_t chan_id)
|
{
|
if (chan_id >= CHANNELS) {
|
return NULL;
|
}
|
|
return &channels[chan_id];
|
}
|
|
static void
|
tester_l2cap_coc_recv(struct ble_l2cap_chan *chan, struct os_mbuf *sdu)
|
{
|
SYS_LOG_DBG("LE CoC SDU received, chan: 0x%08lx, data len %d",
|
(uint32_t) chan, OS_MBUF_PKTLEN(sdu));
|
|
os_mbuf_free_chain(sdu);
|
if (!hold_credit) {
|
sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
|
assert(sdu != NULL);
|
|
|
ble_l2cap_recv_ready(chan, sdu);
|
}
|
}
|
|
static void recv_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
|
struct os_mbuf *buf, void *arg)
|
{
|
struct l2cap_data_received_ev *ev = (void *) recv_cb_buf;
|
struct channel *channel = find_channel(chan);
|
assert(channel != NULL);
|
|
ev->chan_id = channel->chan_id;
|
ev->data_length = OS_MBUF_PKTLEN(buf);
|
|
if (ev->data_length > TESTER_COC_MTU) {
|
SYS_LOG_ERR("Too large sdu received, truncating data");
|
ev->data_length = TESTER_COC_MTU;
|
}
|
os_mbuf_copydata(buf, 0, ev->data_length, ev->data);
|
|
tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DATA_RECEIVED,
|
CONTROLLER_INDEX, recv_cb_buf, sizeof(*ev) + ev->data_length);
|
|
tester_l2cap_coc_recv(chan, buf);
|
}
|
|
static void unstalled_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
|
int status, void *arg)
|
{
|
if (status) {
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA,
|
CONTROLLER_INDEX, BTP_STATUS_FAILED);
|
} else {
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA,
|
CONTROLLER_INDEX, BTP_STATUS_SUCCESS);
|
}
|
}
|
|
static void reconfigured_ev(uint16_t conn_handle, struct ble_l2cap_chan *chan,
|
struct ble_l2cap_chan_info *chan_info,
|
int status)
|
{
|
struct l2cap_reconfigured_ev ev;
|
struct channel *channel;
|
|
if (status != 0) {
|
return;
|
}
|
|
channel = find_channel(chan);
|
assert(channel != NULL);
|
|
ev.chan_id = channel->chan_id;
|
ev.peer_mtu = chan_info->peer_coc_mtu;
|
ev.peer_mps = chan_info->peer_l2cap_mtu;
|
ev.our_mtu = chan_info->our_coc_mtu;
|
ev.our_mps = chan_info->our_l2cap_mtu;
|
|
tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_RECONFIGURED,
|
CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev));
|
}
|
|
static void connected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
|
struct ble_l2cap_chan_info *chan_info, void *arg)
|
{
|
struct l2cap_connected_ev ev;
|
struct ble_gap_conn_desc desc;
|
struct channel *channel = find_channel(chan);
|
|
if (channel == NULL) {
|
channel = get_free_channel();
|
}
|
|
ev.chan_id = channel->chan_id;
|
ev.psm = chan_info->psm;
|
ev.peer_mtu = chan_info->peer_coc_mtu;
|
ev.peer_mps = chan_info->peer_l2cap_mtu;
|
ev.our_mtu = chan_info->our_coc_mtu;
|
ev.our_mps = chan_info->our_l2cap_mtu;
|
channel->state = 1;
|
channel->chan = chan;
|
|
if (!ble_gap_conn_find(conn_handle, &desc)) {
|
ev.address_type = desc.peer_ota_addr.type;
|
memcpy(ev.address, desc.peer_ota_addr.val,
|
sizeof(ev.address));
|
}
|
|
tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_CONNECTED, CONTROLLER_INDEX,
|
(uint8_t *) &ev, sizeof(ev));
|
}
|
|
static void disconnected_cb(uint16_t conn_handle, struct ble_l2cap_chan *chan,
|
struct ble_l2cap_chan_info *chan_info, void *arg)
|
{
|
struct l2cap_disconnected_ev ev;
|
struct ble_gap_conn_desc desc;
|
struct channel *channel;
|
|
memset(&ev, 0, sizeof(struct l2cap_disconnected_ev));
|
|
channel = find_channel(chan);
|
assert(channel != NULL);
|
|
channel->state = 0;
|
channel->chan = chan;
|
ev.chan_id = channel->chan_id;
|
ev.psm = chan_info->psm;
|
|
if (!ble_gap_conn_find(conn_handle, &desc)) {
|
ev.address_type = desc.peer_ota_addr.type;
|
memcpy(ev.address, desc.peer_ota_addr.val,
|
sizeof(ev.address));
|
}
|
|
tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_EV_DISCONNECTED,
|
CONTROLLER_INDEX, (uint8_t *) &ev, sizeof(ev));
|
}
|
|
static int accept_cb(uint16_t conn_handle, uint16_t peer_mtu,
|
struct ble_l2cap_chan *chan)
|
{
|
struct os_mbuf *sdu_rx;
|
|
SYS_LOG_DBG("LE CoC accepting, chan: 0x%08lx, peer_mtu %d",
|
(uint32_t) chan, peer_mtu);
|
|
sdu_rx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
|
if (!sdu_rx) {
|
return BLE_HS_ENOMEM;
|
}
|
|
ble_l2cap_recv_ready(chan, sdu_rx);
|
|
return 0;
|
}
|
|
static int
|
tester_l2cap_event(struct ble_l2cap_event *event, void *arg)
|
{
|
struct ble_l2cap_chan_info chan_info;
|
int accept_response;
|
struct ble_gap_conn_desc conn;
|
|
switch (event->type) {
|
case BLE_L2CAP_EVENT_COC_CONNECTED:
|
if (ble_l2cap_get_chan_info(event->connect.chan, &chan_info)) {
|
assert(0);
|
}
|
|
if (event->connect.status) {
|
console_printf("LE COC error: %d\n", event->connect.status);
|
return 0;
|
}
|
|
console_printf("LE COC connected, conn: %d, chan: 0x%08lx, "
|
"psm: 0x%02x, scid: 0x%04x, dcid: 0x%04x, "
|
"our_mps: %d, our_mtu: %d, peer_mps: %d, "
|
"peer_mtu: %d\n", event->connect.conn_handle,
|
(uint32_t) event->connect.chan, chan_info.psm,
|
chan_info.scid, chan_info.dcid,
|
chan_info.our_l2cap_mtu, chan_info.our_coc_mtu,
|
chan_info.peer_l2cap_mtu, chan_info.peer_coc_mtu);
|
|
connected_cb(event->connect.conn_handle,
|
event->connect.chan, &chan_info, arg);
|
|
return 0;
|
case BLE_L2CAP_EVENT_COC_DISCONNECTED:
|
if (ble_l2cap_get_chan_info(event->disconnect.chan,
|
&chan_info)) {
|
assert(0);
|
}
|
console_printf("LE CoC disconnected, chan: 0x%08lx\n",
|
(uint32_t) event->disconnect.chan);
|
|
disconnected_cb(event->disconnect.conn_handle,
|
event->disconnect.chan, &chan_info, arg);
|
return 0;
|
case BLE_L2CAP_EVENT_COC_ACCEPT:
|
ble_l2cap_get_chan_info(event->accept.chan, &chan_info);
|
if (chan_info.psm == 0x00F2) {
|
/* TSPX_psm_authentication_required */
|
ble_gap_conn_find(event->accept.conn_handle, &conn);
|
if (!conn.sec_state.authenticated) {
|
return BLE_HS_EAUTHEN;
|
}
|
} else if (chan_info.psm == 0x00F3) {
|
/* TSPX_psm_authorization_required */
|
ble_gap_conn_find(event->accept.conn_handle, &conn);
|
if (!conn.sec_state.encrypted) {
|
return BLE_HS_EAUTHOR;
|
}
|
return BLE_HS_EAUTHOR;
|
} else if (chan_info.psm == 0x00F4) {
|
/* TSPX_psm_encryption_key_size_required */
|
ble_gap_conn_find(event->accept.conn_handle, &conn);
|
if (conn.sec_state.key_size < 16) {
|
return BLE_HS_EENCRYPT_KEY_SZ;
|
}
|
}
|
|
accept_response = POINTER_TO_INT(arg);
|
if (accept_response) {
|
return accept_response;
|
}
|
|
console_printf("LE CoC accept, chan: 0x%08lx, handle: %u, sdu_size: %u\n",
|
(uint32_t) event->accept.chan,
|
event->accept.conn_handle,
|
event->accept.peer_sdu_size);
|
|
return accept_cb(event->accept.conn_handle,
|
event->accept.peer_sdu_size,
|
event->accept.chan);
|
|
case BLE_L2CAP_EVENT_COC_DATA_RECEIVED:
|
console_printf("LE CoC data received, chan: 0x%08lx, handle: %u, sdu_len: %u\n",
|
(uint32_t) event->receive.chan,
|
event->receive.conn_handle,
|
OS_MBUF_PKTLEN(event->receive.sdu_rx));
|
|
recv_cb(event->receive.conn_handle, event->receive.chan,
|
event->receive.sdu_rx, arg);
|
return 0;
|
case BLE_L2CAP_EVENT_COC_TX_UNSTALLED:
|
console_printf("LE CoC tx unstalled, chan: 0x%08lx, handle: %u, status: %d\n",
|
(uint32_t) event->tx_unstalled.chan,
|
event->tx_unstalled.conn_handle,
|
event->tx_unstalled.status);
|
|
unstalled_cb(event->tx_unstalled.conn_handle,
|
event->tx_unstalled.chan,
|
event->tx_unstalled.status, arg);
|
return 0;
|
case BLE_L2CAP_EVENT_COC_RECONFIG_COMPLETED:
|
if (ble_l2cap_get_chan_info(event->reconfigured.chan,
|
&chan_info)) {
|
assert(0);
|
}
|
console_printf("LE CoC reconfigure completed status 0x%02x, "
|
"chan: 0x%08lx\n", event->reconfigured.status,
|
(uint32_t) event->reconfigured.chan);
|
|
if (event->reconfigured.status == 0) {
|
console_printf("\t our_mps: %d our_mtu %d\n",
|
chan_info.our_l2cap_mtu, chan_info.our_coc_mtu);
|
}
|
|
reconfigured_ev(event->reconfigured.conn_handle,
|
event->reconfigured.chan,
|
&chan_info,
|
event->reconfigured.status);
|
return 0;
|
case BLE_L2CAP_EVENT_COC_PEER_RECONFIGURED:
|
if (ble_l2cap_get_chan_info(event->reconfigured.chan,
|
&chan_info)) {
|
assert(0);
|
}
|
console_printf("LE CoC peer reconfigured status 0x%02x, "
|
"chan: 0x%08lx\n", event->reconfigured.status,
|
(uint32_t) event->reconfigured.chan);
|
|
if (event->reconfigured.status == 0) {
|
console_printf("\t peer_mps: %d peer_mtu %d\n",
|
chan_info.peer_l2cap_mtu, chan_info.peer_coc_mtu);
|
}
|
|
reconfigured_ev(event->reconfigured.conn_handle,
|
event->reconfigured.chan,
|
&chan_info,
|
event->reconfigured.status);
|
return 0;
|
default:
|
return 0;
|
}
|
}
|
|
static void connect(uint8_t *data, uint16_t len)
|
{
|
const struct l2cap_connect_cmd *cmd = (void *) data;
|
uint8_t rp_buf[sizeof(struct l2cap_connect_rp) + cmd->num];
|
struct l2cap_connect_rp *rp = (void *) rp_buf;
|
struct ble_gap_conn_desc desc;
|
struct channel *chan;
|
struct os_mbuf *sdu_rx[cmd->num];
|
ble_addr_t *addr = (void *) data;
|
uint16_t mtu = htole16(cmd->mtu);
|
int rc;
|
int i, j;
|
bool ecfc = cmd->options & L2CAP_CONNECT_OPT_ECFC;
|
hold_credit = cmd->options & L2CAP_CONNECT_OPT_HOLD_CREDIT;
|
|
SYS_LOG_DBG("connect: type: %d addr: %s", addr->type, bt_hex(addr->val, 6));
|
|
if (mtu == 0 || mtu > TESTER_COC_MTU) {
|
mtu = TESTER_COC_MTU;
|
}
|
|
rc = ble_gap_conn_find_by_addr(addr, &desc);
|
if (rc) {
|
SYS_LOG_ERR("GAP conn find failed");
|
goto fail;
|
}
|
|
rp->num = cmd->num;
|
|
for (i = 0; i < cmd->num; i++) {
|
chan = get_free_channel();
|
if (!chan) {
|
SYS_LOG_ERR("No free channels");
|
goto fail;
|
}
|
/* temporarily mark channel as used to select next one */
|
chan->state = 1;
|
|
rp->chan_ids[i] = chan->chan_id;
|
|
sdu_rx[i] = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
|
if (sdu_rx[i] == NULL) {
|
SYS_LOG_ERR("Failed to alloc buf");
|
goto fail;
|
}
|
}
|
|
/* mark selected channels as unused again */
|
for (i = 0; i < cmd->num; i++) {
|
for (j = 0; j < CHANNELS; j++) {
|
if (rp->chan_ids[i] == channels[j].chan_id) {
|
channels[j].state = 0;
|
}
|
}
|
}
|
|
if (cmd->num == 1 && !ecfc) {
|
rc = ble_l2cap_connect(desc.conn_handle, htole16(cmd->psm),
|
mtu, sdu_rx[0],
|
tester_l2cap_event, NULL);
|
} else if (ecfc) {
|
rc = ble_l2cap_enhanced_connect(desc.conn_handle,
|
htole16(cmd->psm), mtu,
|
cmd->num, sdu_rx,
|
tester_l2cap_event, NULL);
|
} else {
|
SYS_LOG_ERR("Invalid 'num' parameter value");
|
goto fail;
|
}
|
|
if (rc) {
|
SYS_LOG_ERR("L2CAP connect failed\n");
|
goto fail;
|
}
|
|
tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX,
|
(uint8_t *) rp, sizeof(rp_buf));
|
|
return;
|
|
fail:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CONNECT, CONTROLLER_INDEX,
|
BTP_STATUS_FAILED);
|
}
|
|
static void disconnect(const uint8_t *data, uint16_t len)
|
{
|
const struct l2cap_disconnect_cmd *cmd = (void *) data;
|
struct channel *chan;
|
uint8_t status;
|
int err;
|
|
SYS_LOG_DBG("");
|
|
chan = get_channel(cmd->chan_id);
|
assert(chan != NULL);
|
|
err = ble_l2cap_disconnect(chan->chan);
|
if (err) {
|
status = BTP_STATUS_FAILED;
|
goto rsp;
|
}
|
|
status = BTP_STATUS_SUCCESS;
|
|
rsp:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_DISCONNECT, CONTROLLER_INDEX,
|
status);
|
}
|
|
static void send_data(const uint8_t *data, uint16_t len)
|
{
|
const struct l2cap_send_data_cmd *cmd = (void *) data;
|
struct os_mbuf *sdu_tx = NULL;
|
int rc;
|
uint16_t data_len = sys_le16_to_cpu(cmd->data_len);
|
struct channel *chan = get_channel(cmd->chan_id);
|
|
SYS_LOG_DBG("cmd->chan_id=%d", cmd->chan_id);
|
|
if (!chan) {
|
SYS_LOG_ERR("Invalid channel\n");
|
goto fail;
|
}
|
|
/* FIXME: For now, fail if data length exceeds buffer length */
|
if (data_len > TESTER_COC_MTU) {
|
SYS_LOG_ERR("Data length exceeds buffer length");
|
goto fail;
|
}
|
|
sdu_tx = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
|
if (sdu_tx == NULL) {
|
SYS_LOG_ERR("No memory in the test sdu pool\n");
|
goto fail;
|
}
|
|
os_mbuf_append(sdu_tx, cmd->data, data_len);
|
|
/* ble_l2cap_send takes ownership of the sdu */
|
rc = ble_l2cap_send(chan->chan, sdu_tx);
|
if (rc == 0 || rc == BLE_HS_ESTALLED) {
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX,
|
BTP_STATUS_SUCCESS);
|
return;
|
}
|
|
SYS_LOG_ERR("Unable to send data: %d", rc);
|
os_mbuf_free_chain(sdu_tx);
|
|
fail:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_SEND_DATA, CONTROLLER_INDEX,
|
BTP_STATUS_FAILED);
|
}
|
|
static int
|
l2cap_coc_err2hs_err(uint16_t coc_err)
|
{
|
switch (coc_err) {
|
case BLE_L2CAP_COC_ERR_UNKNOWN_LE_PSM:
|
return BLE_HS_ENOTSUP;
|
case BLE_L2CAP_COC_ERR_NO_RESOURCES:
|
return BLE_HS_ENOMEM;
|
case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN:
|
return BLE_HS_EAUTHEN;
|
case BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR:
|
return BLE_HS_EAUTHOR;
|
case BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC:
|
return BLE_HS_EENCRYPT;
|
case BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ:
|
return BLE_HS_EENCRYPT_KEY_SZ;
|
case BLE_L2CAP_COC_ERR_UNACCEPTABLE_PARAMETERS:
|
return BLE_HS_EINVAL;
|
default:
|
return 0;
|
}
|
}
|
|
static int
|
l2cap_btp_listen_err2coc_err(uint16_t coc_err)
|
{
|
switch (coc_err) {
|
case 0x01:
|
return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHEN;
|
case 0x02:
|
return BLE_L2CAP_COC_ERR_INSUFFICIENT_AUTHOR;
|
case 0x03:
|
return BLE_L2CAP_COC_ERR_INSUFFICIENT_KEY_SZ;
|
case 0x04:
|
return BLE_L2CAP_COC_ERR_INSUFFICIENT_ENC;
|
default:
|
return 0;
|
}
|
}
|
|
static void listen(const uint8_t *data, uint16_t len)
|
{
|
const struct l2cap_listen_cmd *cmd = (void *) data;
|
uint16_t mtu = htole16(cmd->mtu);
|
uint16_t rsp = htole16(cmd->response);
|
int rc;
|
|
SYS_LOG_DBG("");
|
|
if (mtu == 0 || mtu > TESTER_COC_MTU) {
|
mtu = TESTER_COC_MTU;
|
}
|
|
rsp = l2cap_btp_listen_err2coc_err(rsp);
|
rsp = l2cap_coc_err2hs_err(rsp);
|
|
/* TODO: Handle cmd->transport flag */
|
rc = ble_l2cap_create_server(cmd->psm, mtu, tester_l2cap_event,
|
INT_TO_POINTER(rsp));
|
if (rc) {
|
goto fail;
|
}
|
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX,
|
BTP_STATUS_SUCCESS);
|
return;
|
|
fail:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_LISTEN, CONTROLLER_INDEX,
|
BTP_STATUS_FAILED);
|
}
|
|
static void credits(uint8_t *data, uint16_t len)
|
{
|
const struct l2cap_credits_cmd *cmd = (void *)data;
|
struct os_mbuf *sdu;
|
int rc;
|
|
struct channel *channel = get_channel(cmd->chan_id);
|
if (channel == NULL) {
|
goto fail;
|
}
|
|
sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
|
if (sdu == NULL) {
|
os_mbuf_free_chain(sdu);
|
goto fail;
|
}
|
|
rc = ble_l2cap_recv_ready(channel->chan, sdu);
|
if (rc != 0) {
|
goto fail;
|
}
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CREDITS, CONTROLLER_INDEX,
|
BTP_STATUS_SUCCESS);
|
return;
|
fail:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_CREDITS, CONTROLLER_INDEX,
|
BTP_STATUS_FAILED);
|
}
|
|
static void reconfigure(const uint8_t *data, uint16_t len)
|
{
|
const struct l2cap_reconfigure_cmd *cmd = (void *) data;
|
uint16_t mtu = htole16(cmd->mtu);
|
struct ble_gap_conn_desc desc;
|
ble_addr_t *addr = (void *) data;
|
struct ble_l2cap_chan *chans[cmd->num];
|
struct channel *channel;
|
int rc;
|
int i;
|
|
SYS_LOG_DBG("");
|
|
if (mtu == 0 || mtu > TESTER_COC_MTU) {
|
mtu = TESTER_COC_MTU;
|
}
|
|
rc = ble_gap_conn_find_by_addr(addr, &desc);
|
if (rc) {
|
SYS_LOG_ERR("GAP conn find failed");
|
goto fail;
|
}
|
|
for (i = 0; i < cmd->num; ++i) {
|
channel = get_channel(cmd->idxs[i]);
|
if (channel == NULL) {
|
goto fail;
|
}
|
chans[i] = channel->chan;
|
}
|
|
rc = ble_l2cap_reconfig(chans, cmd->num, mtu);
|
if (rc) {
|
goto fail;
|
}
|
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_RECONFIGURE, CONTROLLER_INDEX,
|
BTP_STATUS_SUCCESS);
|
return;
|
|
fail:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, L2CAP_RECONFIGURE, CONTROLLER_INDEX,
|
BTP_STATUS_FAILED);
|
}
|
|
static void supported_commands(uint8_t *data, uint16_t len)
|
{
|
uint8_t cmds[1];
|
struct l2cap_read_supported_commands_rp *rp = (void *) cmds;
|
|
memset(cmds, 0, sizeof(cmds));
|
|
tester_set_bit(cmds, L2CAP_READ_SUPPORTED_COMMANDS);
|
tester_set_bit(cmds, L2CAP_CONNECT);
|
tester_set_bit(cmds, L2CAP_DISCONNECT);
|
tester_set_bit(cmds, L2CAP_LISTEN);
|
tester_set_bit(cmds, L2CAP_SEND_DATA);
|
tester_set_bit(cmds, L2CAP_RECONFIGURE);
|
|
tester_send(BTP_SERVICE_ID_L2CAP, L2CAP_READ_SUPPORTED_COMMANDS,
|
CONTROLLER_INDEX, (uint8_t *) rp, sizeof(cmds));
|
}
|
|
void tester_handle_l2cap(uint8_t opcode, uint8_t index, uint8_t *data,
|
uint16_t len)
|
{
|
switch (opcode) {
|
case L2CAP_READ_SUPPORTED_COMMANDS:
|
supported_commands(data, len);
|
return;
|
case L2CAP_CONNECT:
|
connect(data, len);
|
return;
|
case L2CAP_DISCONNECT:
|
disconnect(data, len);
|
return;
|
case L2CAP_SEND_DATA:
|
send_data(data, len);
|
return;
|
case L2CAP_LISTEN:
|
listen(data, len);
|
return;
|
case L2CAP_RECONFIGURE:
|
reconfigure(data, len);
|
return;
|
case L2CAP_CREDITS:
|
credits(data, len);
|
return;
|
default:
|
tester_rsp(BTP_SERVICE_ID_L2CAP, opcode, index,
|
BTP_STATUS_UNKNOWN_CMD);
|
return;
|
}
|
}
|
|
uint8_t tester_init_l2cap(void)
|
{
|
int rc;
|
|
/* For testing we want to support all the available channels */
|
rc = os_mempool_init(&sdu_coc_mbuf_mempool, TESTER_COC_BUF_COUNT,
|
TESTER_COC_MTU, tester_sdu_coc_mem,
|
"tester_coc_sdu_pool");
|
assert(rc == 0);
|
|
rc = os_mbuf_pool_init(&sdu_os_mbuf_pool, &sdu_coc_mbuf_mempool,
|
TESTER_COC_MTU, TESTER_COC_BUF_COUNT);
|
assert(rc == 0);
|
|
return BTP_STATUS_SUCCESS;
|
}
|
|
uint8_t tester_unregister_l2cap(void)
|
{
|
return BTP_STATUS_SUCCESS;
|
}
|
|
#endif
|
