apps/bttester: refactor L2CAP service

Accomodated to new API.
This commit is contained in:
Krzysztof Kopyściński
2023-07-26 19:38:24 +02:00
committed by Szymon Janc
parent 6f34f7c923
commit 2210018c04
2 changed files with 204 additions and 172 deletions
-3
View File
@@ -120,9 +120,6 @@ uint8_t
tester_init_l2cap(void);
uint8_t
tester_unregister_l2cap(void);
void
tester_handle_l2cap(uint8_t opcode, uint8_t *data,
uint16_t len);
#endif
#if MYNEWT_VAL(BLE_MESH)
+204 -169
View File
@@ -72,7 +72,6 @@ get_free_channel(void)
chan = &channels[i];
chan->chan_id = i;
return chan;
}
@@ -399,43 +398,45 @@ tester_l2cap_event(struct ble_l2cap_event *event, void *arg)
}
}
static void
connect(uint8_t *data, uint16_t len)
static uint8_t
connect(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
const struct btp_l2cap_connect_cmd *cmd = (void *) data;
uint8_t rp_buf[sizeof(struct btp_l2cap_connect_rp) + cmd->num];
struct btp_l2cap_connect_rp *rp = (void *) rp_buf;
const struct btp_l2cap_connect_cmd *cp = cmd;
struct btp_l2cap_connect_rp *rp = rsp;
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);
struct os_mbuf *sdu_rx[cp->num];
ble_addr_t *addr = (void *)&cp->address;
uint16_t mtu = le16toh(cp->mtu);
uint16_t psm = le16toh(cp->psm);
int rc;
int i, j;
bool ecfc = cmd->options & BTP_L2CAP_CONNECT_OPT_ECFC;
hold_credit = cmd->options & BTP_L2CAP_CONNECT_OPT_HOLD_CREDIT;
uint8_t status = BTP_STATUS_SUCCESS;
bool ecfc = cp->options & BTP_L2CAP_CONNECT_OPT_ECFC;
hold_credit = cp->options & BTP_L2CAP_CONNECT_OPT_HOLD_CREDIT;
SYS_LOG_DBG("connect: type: %d addr: %s",
addr->type,
string_from_bytes(addr->val, 6));
if (mtu == 0 || mtu > TESTER_COC_MTU) {
mtu = TESTER_COC_MTU;
rc = ble_gap_conn_find_by_addr(addr, &desc);
if (cp->num == 0 || cp->num > CHANNELS ||
mtu > TESTER_COC_MTU || mtu == 0) {
return BTP_STATUS_FAILED;
}
rc = ble_gap_conn_find_by_addr(addr, &desc);
if (rc) {
SYS_LOG_ERR("GAP conn find failed");
goto fail;
return BTP_STATUS_FAILED;
}
rp->num = cmd->num;
for (i = 0; i < cmd->num; i++) {
for (i = 0; i < cp->num; i++) {
chan = get_free_channel();
if (!chan) {
SYS_LOG_ERR("No free channels");
goto fail;
status = BTP_STATUS_FAILED;
goto done;
}
/* temporarily mark channel as used to select next one */
chan->state = 1;
@@ -445,12 +446,38 @@ connect(uint8_t *data, uint16_t len)
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;
status = BTP_STATUS_FAILED;
goto done;
}
}
if (cp->num == 1 && !ecfc) {
rc = ble_l2cap_connect(desc.conn_handle, psm,
mtu, sdu_rx[0],
tester_l2cap_event, NULL);
} else if (ecfc) {
rc = ble_l2cap_enhanced_connect(desc.conn_handle,
psm, mtu,
cp->num, sdu_rx,
tester_l2cap_event, NULL);
} else {
SYS_LOG_ERR("Invalid 'num' parameter value");
status = BTP_STATUS_FAILED;
goto done;
}
if (rc) {
SYS_LOG_ERR("L2CAP connect failed\n");
status = BTP_STATUS_FAILED;
goto done;
}
rp->num = cp->num;
*rsp_len = sizeof(*rp) + (rp->num * sizeof(rp->chan_ids[0]));
done:
/* mark selected channels as unused again */
for (i = 0; i < cmd->num; i++) {
for (i = 0; i < cp->num; i++) {
for (j = 0; j < CHANNELS; j++) {
if (rp->chan_ids[i] == channels[j].chan_id) {
channels[j].state = 0;
@@ -458,173 +485,164 @@ connect(uint8_t *data, uint16_t len)
}
}
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, BTP_L2CAP_CONNECT,
(uint8_t *) rp, sizeof(rp_buf));
return;
fail:
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_CONNECT,
BTP_STATUS_FAILED);
return status;
}
static void
disconnect(const uint8_t *data, uint16_t len)
static uint8_t
disconnect(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
const struct btp_l2cap_disconnect_cmd *cmd = (void *) data;
const struct btp_l2cap_disconnect_cmd *cp = cmd;
struct channel *chan;
uint8_t status;
int err;
SYS_LOG_DBG("");
chan = get_channel(cmd->chan_id);
if (cp->chan_id >= CHANNELS) {
return BTP_STATUS_FAILED;
}
chan = get_channel(cp->chan_id);
assert(chan != NULL);
err = ble_l2cap_disconnect(chan->chan);
if (err) {
status = BTP_STATUS_FAILED;
goto rsp;
return BTP_STATUS_FAILED;
}
status = BTP_STATUS_SUCCESS;
rsp:
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_DISCONNECT,
status);
return BTP_STATUS_SUCCESS;
}
static void
send_data(const uint8_t *data, uint16_t len)
static uint8_t
send_data(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
const struct btp_l2cap_send_data_cmd *cmd = (void *) data;
const struct btp_l2cap_send_data_cmd *cp = cmd;
struct channel *chan;
struct os_mbuf *sdu_tx = NULL;
int rc;
uint16_t data_len = le16toh(cmd->data_len);
struct channel *chan = get_channel(cmd->chan_id);
uint16_t data_len;
SYS_LOG_DBG("cmd->chan_id=%d", cmd->chan_id);
SYS_LOG_DBG("cmd->chan_id=%d", cp->chan_id);
if (cmd_len < sizeof(*cp) ||
cmd_len != sizeof(*cp) + le16toh(cp->data_len)) {
return BTP_STATUS_FAILED;
}
if (cp->chan_id >= CHANNELS) {
return BTP_STATUS_FAILED;
}
chan = get_channel(cp->chan_id);
data_len = le16toh(cp->data_len);
if (!chan) {
SYS_LOG_ERR("Invalid channel\n");
goto fail;
return BTP_STATUS_FAILED;
}
/* 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;
return BTP_STATUS_FAILED;
}
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");
rc = BLE_HS_ENOMEM;
goto fail;
}
os_mbuf_append(sdu_tx, cmd->data, data_len);
os_mbuf_append(sdu_tx, cp->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, BTP_L2CAP_SEND_DATA,
BTP_STATUS_SUCCESS);
return;
return BTP_STATUS_SUCCESS;
}
fail:
SYS_LOG_ERR("Unable to send data: %d", rc);
os_mbuf_free_chain(sdu_tx);
fail:
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_SEND_DATA,
BTP_STATUS_FAILED);
return BTP_STATUS_FAILED;
}
static void
listen(const uint8_t *data, uint16_t len)
static uint8_t
listen(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
const struct btp_l2cap_listen_cmd *cmd = (void *) data;
uint16_t mtu = htole16(cmd->mtu);
const struct btp_l2cap_listen_cmd *cp = cmd;
uint16_t mtu = htole16(cp->mtu);
uint16_t psm = htole16(cp->psm);
int rc;
SYS_LOG_DBG("");
if (psm == 0) {
return BTP_STATUS_FAILED;
}
if (mtu == 0 || mtu > TESTER_COC_MTU) {
mtu = TESTER_COC_MTU;
}
/* TODO: Handle cmd->transport flag */
rc = ble_l2cap_create_server(cmd->psm, mtu, tester_l2cap_event, NULL);
if (rc) {
goto fail;
/* We do not support BR/EDR transport */
if (cp->transport == 0) {
return BTP_STATUS_FAILED;
}
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_LISTEN,
BTP_STATUS_SUCCESS);
return;
rc = ble_l2cap_create_server(psm, mtu, tester_l2cap_event, NULL);
if (rc) {
return BTP_STATUS_FAILED;
}
fail:
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_LISTEN,
BTP_STATUS_FAILED);
return BTP_STATUS_SUCCESS;
}
static void
credits(uint8_t *data, uint16_t len)
static uint8_t
credits(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
const struct btp_l2cap_credits_cmd *cmd = (void *) data;
const struct btp_l2cap_credits_cmd *cp = cmd;
struct channel *chan;
struct os_mbuf *sdu;
int rc;
struct channel *channel = get_channel(cmd->chan_id);
if (channel == NULL) {
goto fail;
if (cp->chan_id >= CHANNELS) {
return BTP_STATUS_FAILED;
}
chan = get_channel(cp->chan_id);
if (chan == NULL) {
return BTP_STATUS_FAILED;
}
sdu = os_mbuf_get_pkthdr(&sdu_os_mbuf_pool, 0);
if (sdu == NULL) {
os_mbuf_free_chain(sdu);
goto fail;
return BTP_STATUS_FAILED;
}
rc = ble_l2cap_recv_ready(channel->chan, sdu);
rc = ble_l2cap_recv_ready(chan->chan, sdu);
if (rc != 0) {
goto fail;
return BTP_STATUS_FAILED;
}
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_CREDITS,
BTP_STATUS_SUCCESS);
return;
fail:
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_CREDITS,
BTP_STATUS_FAILED);
return BTP_STATUS_SUCCESS;
}
static void
reconfigure(const uint8_t *data, uint16_t len)
static uint8_t
reconfigure(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
const struct btp_l2cap_reconfigure_cmd *cmd = (void *) data;
uint16_t mtu = htole16(cmd->mtu);
const struct btp_l2cap_reconfigure_cmd *cp = cmd;
uint16_t mtu = le16toh(cp->mtu);
struct ble_gap_conn_desc desc;
ble_addr_t *addr = (void *) data;
struct ble_l2cap_chan *chans[cmd->num];
ble_addr_t *addr = (ble_addr_t *)&cp->address;
struct ble_l2cap_chan *chans[cp->num];
struct channel *channel;
int rc;
int i;
@@ -638,82 +656,96 @@ reconfigure(const uint8_t *data, uint16_t len)
rc = ble_gap_conn_find_by_addr(addr, &desc);
if (rc) {
SYS_LOG_ERR("GAP conn find failed");
goto fail;
return BTP_STATUS_FAILED;
}
for (i = 0; i < cmd->num; ++i) {
channel = get_channel(cmd->idxs[i]);
if (cmd_len < sizeof(*cp) ||
cmd_len != sizeof(*cp) + cp->num) {
return BTP_STATUS_FAILED;
}
if (cp->num > CHANNELS) {
return BTP_STATUS_FAILED;
}
mtu = le16toh(cp->mtu);
if (mtu > TESTER_COC_MTU) {
return BTP_STATUS_FAILED;
}
for (i = 0; i < cp->num; ++i) {
channel = get_channel(cp->idxs[i]);
if (channel == NULL) {
goto fail;
return BTP_STATUS_FAILED;
}
chans[i] = channel->chan;
}
rc = ble_l2cap_reconfig(chans, cmd->num, mtu);
rc = ble_l2cap_reconfig(chans, cp->num, mtu);
if (rc) {
goto fail;
return BTP_STATUS_FAILED;
}
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_RECONFIGURE,
BTP_STATUS_SUCCESS);
return;
fail:
tester_rsp(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_RECONFIGURE,
BTP_STATUS_FAILED);
return BTP_STATUS_SUCCESS;
}
static void
supported_commands(uint8_t *data, uint16_t len)
static uint8_t
supported_commands(const void *cmd, uint16_t cmd_len,
void *rsp, uint16_t *rsp_len)
{
uint8_t cmds[1];
struct btp_l2cap_read_supported_commands_rp *rp = (void *) cmds;
struct btp_l2cap_read_supported_commands_rp *rp = rsp;
memset(cmds, 0, sizeof(cmds));
/* octet 0 */
tester_set_bit(rp->data, BTP_L2CAP_READ_SUPPORTED_COMMANDS);
tester_set_bit(rp->data, BTP_L2CAP_CONNECT);
tester_set_bit(rp->data, BTP_L2CAP_DISCONNECT);
tester_set_bit(rp->data, BTP_L2CAP_SEND_DATA);
tester_set_bit(rp->data, BTP_L2CAP_LISTEN);
/* octet 1 */
tester_set_bit(rp->data, BTP_L2CAP_CREDITS);
*rsp_len = sizeof(*rp) + 2;
tester_set_bit(cmds, BTP_L2CAP_READ_SUPPORTED_COMMANDS);
tester_set_bit(cmds, BTP_L2CAP_CONNECT);
tester_set_bit(cmds, BTP_L2CAP_DISCONNECT);
tester_set_bit(cmds, BTP_L2CAP_LISTEN);
tester_set_bit(cmds, BTP_L2CAP_SEND_DATA);
tester_set_bit(cmds, BTP_L2CAP_RECONFIGURE);
tester_send(BTP_SERVICE_ID_L2CAP, BTP_L2CAP_READ_SUPPORTED_COMMANDS,
(uint8_t *) rp, sizeof(cmds));
return BTP_STATUS_SUCCESS;
}
void
tester_handle_l2cap(uint8_t opcode, uint8_t *data,
uint16_t len)
{
switch (opcode) {
case BTP_L2CAP_READ_SUPPORTED_COMMANDS:
supported_commands(data, len);
return;
case BTP_L2CAP_CONNECT:
connect(data, len);
return;
case BTP_L2CAP_DISCONNECT:
disconnect(data, len);
return;
case BTP_L2CAP_SEND_DATA:
send_data(data, len);
return;
case BTP_L2CAP_LISTEN:
listen(data, len);
return;
case BTP_L2CAP_RECONFIGURE:
reconfigure(data, len);
return;
case BTP_L2CAP_CREDITS:
credits(data, len);
return;
default:
tester_rsp(BTP_SERVICE_ID_L2CAP, opcode,
BTP_STATUS_UNKNOWN_CMD);
return;
}
}
static const struct btp_handler handlers[] = {
{
.opcode = BTP_L2CAP_READ_SUPPORTED_COMMANDS,
.index = BTP_INDEX_NONE,
.expect_len = 0,
.func = supported_commands,
},
{
.opcode = BTP_L2CAP_CONNECT,
.expect_len = sizeof(struct btp_l2cap_connect_cmd),
.func = connect,
},
{
.opcode = BTP_L2CAP_DISCONNECT,
.expect_len = sizeof(struct btp_l2cap_disconnect_cmd),
.func = disconnect,
},
{
.opcode = BTP_L2CAP_SEND_DATA,
.expect_len = -1,
.func = send_data,
},
{
.opcode = BTP_L2CAP_LISTEN,
.expect_len = sizeof(struct btp_l2cap_listen_cmd),
.func = listen,
},
{
.opcode = BTP_L2CAP_RECONFIGURE,
.expect_len = -1,
.func = reconfigure,
},
{
.opcode = BTP_L2CAP_CREDITS,
.expect_len = sizeof(struct btp_l2cap_credits_cmd),
.func = credits,
},
};
uint8_t
tester_init_l2cap(void)
@@ -730,6 +762,9 @@ tester_init_l2cap(void)
TESTER_COC_MTU, TESTER_COC_BUF_COUNT);
assert(rc == 0);
tester_register_command_handlers(BTP_SERVICE_ID_L2CAP, handlers,
ARRAY_SIZE(handlers));
return BTP_STATUS_SUCCESS;
}