From: Łukasz Rymanowski Date: Sat, 9 May 2020 00:04:46 +0000 (+0200) Subject: stack/l2c: Add support for credit based connection oriented channels X-Git-Url: http://git.osdn.net/view?a=commitdiff_plain;h=4456dc7c91e1cfe2dd0cfd9154274a573d156b03;p=android-x86%2Fsystem-bt.git stack/l2c: Add support for credit based connection oriented channels This patch adds only support for LE There is new L2CAP API to create connection and reconfigure connection. Implementation will always try to create up to 5 channels if possible. Tag: #feature Bug: 159786353 Sponsor: jpawlowski@ Test: compile & manual Change-Id: I8459a7384ca5771b89b6a5c0bdeeacab86dca5da --- diff --git a/main/shim/l2c_api.cc b/main/shim/l2c_api.cc index 9a510a1b0..ec094646b 100644 --- a/main/shim/l2c_api.cc +++ b/main/shim/l2c_api.cc @@ -148,6 +148,29 @@ bool bluetooth::shim::L2CA_GetPeerLECocConfig(uint16_t lcid, return false; } +bool bluetooth::shim::L2CA_ReconfigCreditBasedConnsReq( + const RawAddress& bd_addr, std::vector& lcids, + tL2CAP_LE_CFG_INFO* p_cfg) { + LOG_INFO("UNIMPLEMENTED %s addr: %s cfg:%p", __func__, + bd_addr.ToString().c_str(), p_cfg); + return false; +} + +std::vector bluetooth::shim::L2CA_ConnectCreditBasedReq( + uint16_t psm, const RawAddress& p_bd_addr, tL2CAP_LE_CFG_INFO* p_cfg) { + LOG_INFO("UNIMPLEMENTED %s addr:%s", __func__, p_bd_addr.ToString().c_str()); + std::vector result; + return result; +} + +bool bluetooth::shim::L2CA_ConnectCreditBasedRsp( + const RawAddress& bd_addr, uint8_t id, + std::vector& accepted_lcids, uint16_t result, + tL2CAP_LE_CFG_INFO* p_cfg) { + LOG_INFO("UNIMPLEMENTED %s addr:%s", __func__, bd_addr.ToString().c_str()); + return false; +} + uint8_t bluetooth::shim::L2CA_DataWrite(uint16_t cid, BT_HDR* p_data) { bool write_success = shim_l2cap.Write(cid, p_data); return write_success ? L2CAP_DW_SUCCESS : L2CAP_DW_FAILED; diff --git a/main/shim/l2c_api.h b/main/shim/l2c_api.h index 3ab1352c4..aeb54ef0a 100644 --- a/main/shim/l2c_api.h +++ b/main/shim/l2c_api.h @@ -18,6 +18,7 @@ #include #include +#include #include "stack/include/l2c_api.h" @@ -176,6 +177,53 @@ bool L2CA_GetPeerLECocConfig(uint16_t lcid, tL2CAP_LE_CFG_INFO* peer_cfg); /******************************************************************************* * + * Function L2CA_ReconfigCreditBasedConnsReq + * + * Description Start reconfigure procedure on Credit Based Connection Oriented + * Channels. + * + * Return value: true if peer is connected + * + ******************************************************************************/ + +bool L2CA_ReconfigCreditBasedConnsReq(const RawAddress& bd_addr, + std::vector& lcids, + tL2CAP_LE_CFG_INFO* p_cfg); + +/******************************************************************************* + * + * Function L2CA_ConnectCreditBasedReq + * + * Description With this function L2CAP will initiate setup of up to 5 credit + * based connections for given psm using provided configuration. + * L2CAP will notify user on the connection result, by calling + * pL2CA_CreditBasedConnectCfm_Cb for each cid with a result. + * + * Return value: vector of allocated local cids for the connection + * + ******************************************************************************/ + +extern std::vector L2CA_ConnectCreditBasedReq( + uint16_t psm, const RawAddress& p_bd_addr, tL2CAP_LE_CFG_INFO* p_cfg); + +/******************************************************************************* + * + * Function L2CA_ConnectCreditBasedRsp + * + * Description Response for the pL2CA_CreditBasedConnectInd_Cb which is the + * indication for peer requesting credit based connection. + * + * Return value: true if peer is connected + * + ******************************************************************************/ + +extern bool L2CA_ConnectCreditBasedRsp(const RawAddress& p_bd_addr, uint8_t id, + std::vector& accepted_lcids, + uint16_t result, + tL2CAP_LE_CFG_INFO* p_cfg); + +/******************************************************************************* + * * Function L2CA_DisconnectReq * * Description Higher layers call this function to disconnect a channel. diff --git a/stack/avct/avct_l2c.cc b/stack/avct/avct_l2c.cc index fd7af8102..40005e128 100644 --- a/stack/avct/avct_l2c.cc +++ b/stack/avct/avct_l2c.cc @@ -48,7 +48,8 @@ const tL2CAP_APPL_INFO avct_l2c_appl = { avct_l2c_config_ind_cback, avct_l2c_config_cfm_cback, avct_l2c_disconnect_ind_cback, avct_l2c_data_ind_cback, avct_l2c_congestion_ind_cback, NULL, - avct_on_l2cap_error, + avct_on_l2cap_error, NULL, + NULL, NULL, }; /******************************************************************************* diff --git a/stack/avct/avct_l2c_br.cc b/stack/avct/avct_l2c_br.cc index e8e999477..1f2781ac4 100644 --- a/stack/avct/avct_l2c_br.cc +++ b/stack/avct/avct_l2c_br.cc @@ -49,7 +49,8 @@ const tL2CAP_APPL_INFO avct_l2c_br_appl = { avct_l2c_br_config_ind_cback, avct_l2c_br_config_cfm_cback, avct_l2c_br_disconnect_ind_cback, avct_l2c_br_data_ind_cback, avct_l2c_br_congestion_ind_cback, NULL, - avct_br_on_l2cap_error, + avct_br_on_l2cap_error, NULL, + NULL, NULL }; /******************************************************************************* diff --git a/stack/avdt/avdt_l2c.cc b/stack/avdt/avdt_l2c.cc index dab5accdc..13b238a74 100644 --- a/stack/avdt/avdt_l2c.cc +++ b/stack/avdt/avdt_l2c.cc @@ -51,7 +51,8 @@ const tL2CAP_APPL_INFO avdt_l2c_appl = { avdt_l2c_config_ind_cback, avdt_l2c_config_cfm_cback, avdt_l2c_disconnect_ind_cback, avdt_l2c_data_ind_cback, avdt_l2c_congestion_ind_cback, NULL, - avdt_on_l2cap_error, + avdt_on_l2cap_error, NULL, + NULL, NULL }; /******************************************************************************* diff --git a/stack/gatt/gatt_main.cc b/stack/gatt/gatt_main.cc index 93830dec5..764f65dec 100644 --- a/stack/gatt/gatt_main.cc +++ b/stack/gatt/gatt_main.cc @@ -75,6 +75,9 @@ static const tL2CAP_APPL_INFO dyn_info = { gatt_l2cif_congest_cback, NULL, gatt_on_l2cap_error, + NULL, + NULL, + NULL }; tGATT_CB gatt_cb; diff --git a/stack/hid/hidd_conn.cc b/stack/hid/hidd_conn.cc index 1e49ec8aa..a7f247f31 100644 --- a/stack/hid/hidd_conn.cc +++ b/stack/hid/hidd_conn.cc @@ -55,7 +55,8 @@ static const tL2CAP_APPL_INFO dev_reg_info = { hidd_l2cif_config_ind, hidd_l2cif_config_cfm, hidd_l2cif_disconnect_ind, hidd_l2cif_data_ind, hidd_l2cif_cong_ind, NULL, - hidd_on_l2cap_error, + hidd_on_l2cap_error, NULL, + NULL, NULL }; /******************************************************************************* diff --git a/stack/hid/hidh_conn.cc b/stack/hid/hidh_conn.cc index bbef4e329..d330ad168 100644 --- a/stack/hid/hidh_conn.cc +++ b/stack/hid/hidh_conn.cc @@ -65,7 +65,8 @@ static const tL2CAP_APPL_INFO hst_reg_info = { hidh_l2cif_config_ind, hidh_l2cif_config_cfm, hidh_l2cif_disconnect_ind, hidh_l2cif_data_ind, hidh_l2cif_cong_ind, NULL, - hidh_on_l2cap_error, + hidh_on_l2cap_error, NULL, + NULL, NULL }; /******************************************************************************* diff --git a/stack/include/l2c_api.h b/stack/include/l2c_api.h index feb0ad8a7..2c138a3e9 100644 --- a/stack/include/l2c_api.h +++ b/stack/include/l2c_api.h @@ -24,6 +24,7 @@ #ifndef L2C_API_H #define L2C_API_H +#include #include #include "bt_target.h" @@ -165,6 +166,7 @@ static_assert(L2CAP_LE_CREDIT_THRESHOLD < L2CAP_LE_CREDIT_DEFAULT, * connection oriented channels. */ struct tL2CAP_LE_CFG_INFO { + uint16_t result; /* Only used in confirm messages */ uint16_t mtu; uint16_t mps; uint16_t credits = L2CAP_LE_CREDIT_DEFAULT; @@ -244,6 +246,39 @@ typedef void(tL2CA_TX_COMPLETE_CB)(uint16_t, uint16_t); */ typedef void(tL2CA_ERROR_CB)(uint16_t, uint16_t); +/* Create credit based connection request callback prototype. Parameters are + * BD Address of remote + * Vector of allocated local cids to accept + * PSM + * Peer MTU + * Identifier that the remote sent + */ +typedef void(tL2CA_CREDIT_BASED_CONNECT_IND_CB)(const RawAddress& bdaddr, + std::vector& lcids, + uint16_t psm, uint16_t peer_mtu, + uint8_t identifier); + +/* Credit based connection confirmation callback prototype. Parameters are + * BD Address of remote + * Connected Local CIDs + * Peer MTU + * Result - 0 = connected, non-zero means CID is not connected + */ +typedef void(tL2CA_CREDIT_BASED_CONNECT_CFM_CB)(const RawAddress& bdaddr, + uint16_t lcid, + uint16_t peer_mtu, + uint16_t result); + +/* Credit based reconfiguration confirm callback prototype. Parameters are + * BD Address of remote + * Local CID assigned to the connection + * Flag indicating if this is local or peer configuration + * Pointer to configuration info + */ +typedef void(tL2CA_CREDIT_BASED_RECONFIG_COMPLETED_CB)( + const RawAddress& bdaddr, uint16_t lcid, bool is_local_cfg, + tL2CAP_LE_CFG_INFO* p_cfg); + /* Define the structure that applications use to register with * L2CAP. This structure includes callback functions. All functions * MUST be provided, with the exception of the "connect pending" @@ -259,6 +294,10 @@ typedef struct { tL2CA_CONGESTION_STATUS_CB* pL2CA_CongestionStatus_Cb; tL2CA_TX_COMPLETE_CB* pL2CA_TxComplete_Cb; tL2CA_ERROR_CB* pL2CA_Error_Cb; + tL2CA_CREDIT_BASED_CONNECT_IND_CB* pL2CA_CreditBasedConnectInd_Cb; + tL2CA_CREDIT_BASED_CONNECT_CFM_CB* pL2CA_CreditBasedConnectCfm_Cb; + tL2CA_CREDIT_BASED_RECONFIG_COMPLETED_CB* + pL2CA_CreditBasedReconfigCompleted_Cb; } tL2CAP_APPL_INFO; /* Define the structure that applications use to create or accept @@ -429,6 +468,51 @@ extern bool L2CA_GetPeerLECocConfig(uint16_t lcid, /******************************************************************************* * + * Function L2CA_ReconfigCreditBasedConnsReq + * + * Description Start reconfigure procedure on Connection Oriented Channel. + * + * Return value: true if peer is connected + * + ******************************************************************************/ + +extern bool L2CA_ReconfigCreditBasedConnsReq(const RawAddress& bd_addr, + std::vector& lcids, + tL2CAP_LE_CFG_INFO* p_cfg); + +/******************************************************************************* + * + * Function L2CA_ConnectCreditBasedReq + * + * Description With this function L2CAP will initiate setup of up to 5 credit + * based connections for given psm using provided configuration. + * L2CAP will notify user on the connection result, by calling + * pL2CA_CreditBasedConnectCfm_Cb for each cid with a result. + * + * Return value: vector of allocated local cids for the connection + * + ******************************************************************************/ + +extern std::vector L2CA_ConnectCreditBasedReq( + uint16_t psm, const RawAddress& p_bd_addr, tL2CAP_LE_CFG_INFO* p_cfg); + +/******************************************************************************* + * + * Function L2CA_ConnectCreditBasedRsp + * + * Description Response for the pL2CA_CreditBasedConnectInd_Cb which is the + * indication for peer requesting credit based connection. + * + * Return value: true if peer is connected + * + ******************************************************************************/ + +extern bool L2CA_ConnectCreditBasedRsp(const RawAddress& p_bd_addr, uint8_t id, + std::vector& accepted_lcids, + uint16_t result, + tL2CAP_LE_CFG_INFO* p_cfg); +/******************************************************************************* + * * Function L2CA_DisconnectReq * * Description Higher layers call this function to disconnect a channel. diff --git a/stack/include/l2cdefs.h b/stack/include/l2cdefs.h index 48bd73ace..deb9dbfec 100644 --- a/stack/include/l2cdefs.h +++ b/stack/include/l2cdefs.h @@ -39,6 +39,11 @@ #define L2CAP_CMD_BLE_CREDIT_BASED_CONN_REQ 0x14 #define L2CAP_CMD_BLE_CREDIT_BASED_CONN_RES 0x15 #define L2CAP_CMD_BLE_FLOW_CTRL_CREDIT 0x16 +/* Enhanced CoC */ +#define L2CAP_CMD_CREDIT_BASED_CONN_REQ 0x17 +#define L2CAP_CMD_CREDIT_BASED_CONN_RES 0x18 +#define L2CAP_CMD_CREDIT_BASED_RECONFIG_REQ 0x19 +#define L2CAP_CMD_CREDIT_BASED_RECONFIG_RES 0x1A /* Define some packet and header lengths */ @@ -79,6 +84,16 @@ /* CID, Credit */ #define L2CAP_CMD_BLE_FLOW_CTRL_CREDIT_LEN 4 +/* LE PSM, MTU, MPS, Initial Credits, SCIDS[] */ +#define L2CAP_CMD_CREDIT_BASED_CONN_REQ_MIN_LEN 8 +/* MTU, MPS, Initial Credits, Result, DCIDS[] */ +#define L2CAP_CMD_CREDIT_BASED_CONN_RES_MIN_LEN 8 + +/* MTU, MPS, DCIDS[] */ +#define L2CAP_CMD_CREDIT_BASED_RECONFIG_REQ_MIN_LEN 4 +/* Result */ +#define L2CAP_CMD_CREDIT_BASED_RECONFIG_RES_LEN 2 + /* Define the packet boundary flags */ #define L2CAP_PKT_START_NON_FLUSHABLE 0 @@ -106,13 +121,23 @@ typedef enum : uint8_t { L2CAP_LE_RESULT_NO_PSM = 2, L2CAP_LE_RESULT_NO_RESOURCES = 4, L2CAP_LE_RESULT_INSUFFICIENT_AUTHENTICATION = 5, + L2CAP_LE_RESULT_INSUFFICIENT_AUTHORIZATION = 6, L2CAP_LE_RESULT_INSUFFICIENT_ENCRYP_KEY_SIZE = 7, L2CAP_LE_RESULT_INSUFFICIENT_ENCRYP = 8, /* We don't like peer device response */ L2CAP_LE_RESULT_INVALID_SOURCE_CID = 9, L2CAP_LE_RESULT_SOURCE_CID_ALREADY_ALLOCATED = 0x0A, + L2CAP_LE_RESULT_UNACCEPTABLE_PARAMETERS = 0x0B, + L2CAP_LE_RESULT_INVALID_PARAMETERS = 0x0C } tL2CAP_LE_RESULT_CODE; +/* Credit based reconfig results code */ +#define L2CAP_RECONFIG_SUCCEED 0 +#define L2CAP_RECONFIG_REDUCTION_MTU_NO_ALLOWED 1 +#define L2CAP_RECONFIG_REDUCTION_MPS_NO_ALLOWED 2 +#define L2CAP_RECONFIG_INVALID_DCID 3 +#define L2CAP_RECONFIG_UNACCAPTED_PARAM 4 + /* Define the L2CAP command reject reason codes */ #define L2CAP_CMD_REJ_NOT_UNDERSTOOD 0 diff --git a/stack/l2cap/l2c_api.cc b/stack/l2cap/l2c_api.cc index ad6f70a05..183cbe4b4 100644 --- a/stack/l2cap/l2c_api.cc +++ b/stack/l2cap/l2c_api.cc @@ -663,6 +663,249 @@ bool L2CA_GetPeerLECocConfig(uint16_t lcid, tL2CAP_LE_CFG_INFO* peer_cfg) { /******************************************************************************* * + * Function L2CA_ConnectCreditBasedRsp + * + * Description Response for the pL2CA_CreditBasedConnectInd_Cb which is the + * indication for peer requesting credit based connection. + * + * Parameters: BD address of the peer + * Identifier of the transaction + * Vector of accepted lcids by upper layer + * L2CAP result + * Local channel configuration + * + * Returns true for success, false for failure + * + ******************************************************************************/ +bool L2CA_ConnectCreditBasedRsp(const RawAddress& p_bd_addr, uint8_t id, + std::vector& accepted_lcids, + uint16_t result, tL2CAP_LE_CFG_INFO* p_cfg) { + if (bluetooth::shim::is_gd_shim_enabled()) { + return bluetooth::shim::L2CA_ConnectCreditBasedRsp( + p_bd_addr, id, accepted_lcids, result, p_cfg); + } + + VLOG(1) << __func__ << " BDA: " << p_bd_addr + << StringPrintf(" num of cids: %d Result: %d", + int(accepted_lcids.size()), +result); + + /* First, find the link control block */ + tL2C_LCB* p_lcb = l2cu_find_lcb_by_bd_addr(p_bd_addr, BT_TRANSPORT_LE); + if (p_lcb == NULL) { + /* No link. Get an LCB and start link establishment */ + L2CAP_TRACE_WARNING("%s no LCB", __func__); + return false; + } + + /* Now, find the channel control block. We kept lead cid. + */ + tL2C_CCB* p_ccb = l2cu_find_ccb_by_cid(p_lcb, p_lcb->pending_lead_cid); + + for (uint16_t cid : accepted_lcids) { + tL2C_CCB* temp_p_ccb = l2cu_find_ccb_by_cid(p_lcb, cid); + if (temp_p_ccb == NULL) { + L2CAP_TRACE_WARNING("%s no CCB", __func__); + return false; + } + + if (p_cfg) { + temp_p_ccb->local_conn_cfg = *p_cfg; + temp_p_ccb->remote_credit_count = p_cfg->credits; + } + } + + /* The IDs must match */ + if (p_ccb->remote_id != id) { + L2CAP_TRACE_WARNING("%s bad id. Expected: %d Got: %d", __func__, + p_ccb->remote_id, id); + return false; + } + + tL2C_CONN_INFO conn_info; + conn_info.lcids = accepted_lcids; + conn_info.bd_addr = p_bd_addr; + conn_info.l2cap_result = result; + + if (accepted_lcids.size() > 0) { + l2c_csm_execute(p_ccb, L2CEVT_L2CA_CREDIT_BASED_CONNECT_RSP, &conn_info); + } else { + l2c_csm_execute(p_ccb, L2CEVT_L2CA_CREDIT_BASED_CONNECT_RSP_NEG, + &conn_info); + } + + return true; +} +/******************************************************************************* + * + * Function L2CA_ConnectCreditBasedReq + * + * Description Initiate Create Credit Based connections. + * + * Parameters: PSM for the L2CAP channel + * BD address of the peer + * Local channel configuration + * + * Return value: Vector of allocated local cids. + * + ******************************************************************************/ + +std::vector L2CA_ConnectCreditBasedReq(uint16_t psm, + const RawAddress& p_bd_addr, + tL2CAP_LE_CFG_INFO* p_cfg) { + if (bluetooth::shim::is_gd_shim_enabled()) { + return bluetooth::shim::L2CA_ConnectCreditBasedReq(psm, p_bd_addr, p_cfg); + } + + VLOG(1) << __func__ << " BDA: " << p_bd_addr + << StringPrintf(" PSM: 0x%04x", psm); + + std::vector allocated_cids; + + /* Fail if we have not established communications with the controller */ + if (!BTM_IsDeviceUp()) { + L2CAP_TRACE_WARNING("%s BTU not ready", __func__); + return allocated_cids; + } + + if (!p_cfg) { + L2CAP_TRACE_WARNING("%s p_cfg is NULL", __func__); + return allocated_cids; + } + + /* Fail if the PSM is not registered */ + tL2C_RCB* p_rcb = l2cu_find_ble_rcb_by_psm(psm); + if (p_rcb == NULL) { + L2CAP_TRACE_WARNING("%s No BLE RCB, PSM: 0x%04x", __func__, psm); + return allocated_cids; + } + + /* First, see if we already have a le link to the remote */ + tL2C_LCB* p_lcb = l2cu_find_lcb_by_bd_addr(p_bd_addr, BT_TRANSPORT_LE); + if (p_lcb == NULL || (p_lcb->link_state != LST_CONNECTED)) { + L2CAP_TRACE_WARNING("%s incorrect link state: %d", __func__, + p_lcb->link_state); + return allocated_cids; + } + + L2CAP_TRACE_DEBUG("%s LE Link is up", __func__); + + tL2C_CCB* p_ccb_primary; + + for (int i = 0; i < 5; i++) { + /* Allocate a channel control block */ + tL2C_CCB* p_ccb = l2cu_allocate_ccb(p_lcb, 0); + if (p_ccb == NULL) { + if (i == 0) { + L2CAP_TRACE_WARNING("%s no CCB, PSM: 0x%04x", __func__, psm); + return allocated_cids; + } else { + break; + } + } + + p_ccb->ecoc = true; + p_ccb->local_conn_cfg = *p_cfg; + p_ccb->remote_credit_count = p_cfg->credits; + /* Save registration info */ + p_ccb->p_rcb = p_rcb; + if (i == 0) { + p_ccb_primary = p_ccb; + } else { + /* Only primary channel we keep in closed state, as in that + * context we will run state machine where security is checked etc. + * Others we can directly put into waiting for connect + * response, so those are not confused by system as incomming connections + */ + p_ccb->chnl_state = CST_W4_L2CAP_CONNECT_RSP; + } + + allocated_cids.push_back(p_ccb->local_cid); + } + + p_lcb->pending_ecoc_connection_cids = allocated_cids; + l2c_csm_execute(p_ccb_primary, L2CEVT_L2CA_CREDIT_BASED_CONNECT_REQ, NULL); + + L2CAP_TRACE_API("%s(psm: 0x%04x) returned CID: 0x%04x", __func__, psm, + p_ccb_primary->local_cid); + + return allocated_cids; +} + +/******************************************************************************* + * + * Function L2CA_ReconfigCreditBasedConnsReq + * + * Description Start reconfigure procedure on Connection Oriented Channel. + * + * Parameters: Vector of channels for which configuration should be changed + * New local channel configuration + * + * Return value: true if peer is connected + * + ******************************************************************************/ + +bool L2CA_ReconfigCreditBasedConnsReq(const RawAddress& bda, + std::vector& lcids, + tL2CAP_LE_CFG_INFO* p_cfg) { + if (bluetooth::shim::is_gd_shim_enabled()) { + return bluetooth::shim::L2CA_ReconfigCreditBasedConnsReq(bda, lcids, p_cfg); + } + + tL2C_CCB* p_ccb; + + L2CAP_TRACE_API("L2CA_ReconfigCreditBasedConnsReq() "); + + for (uint16_t cid : lcids) { + p_ccb = l2cu_find_ccb_by_cid(NULL, cid); + + if (!p_ccb) { + L2CAP_TRACE_WARNING("L2CAP - no CCB for L2CA_cfg_req, CID: %d", cid); + return (false); + } + + if ((p_ccb->local_conn_cfg.mtu > p_cfg->mtu) || + (p_ccb->local_conn_cfg.mps > p_cfg->mps)) { + L2CAP_TRACE_WARNING("L2CAP - MPS or MTU reduction, CID: %d", cid); + return (false); + } + } + + if (p_cfg->mtu > L2CAP_MTU_SIZE) { + L2CAP_TRACE_WARNING("L2CAP - adjust MTU: %u too large", p_cfg->mtu); + p_cfg->mtu = L2CAP_MTU_SIZE; + } + + /* Mark all the p_ccbs which going to be reconfigured */ + for (uint16_t cid : lcids) { + L2CAP_TRACE_API(" cid: %d", cid); + p_ccb = l2cu_find_ccb_by_cid(NULL, cid); + if (!p_ccb) { + LOG(ERROR) << __func__ << "Missing cid? " << int(cid); + return (false); + } + p_ccb->reconfig_started = true; + } + + tL2C_LCB* p_lcb = p_ccb->p_lcb; + + /* Hack warning - the whole reconfig we are doing in the context of the first + * p_ccb. In the p_lcp we store configuration and cid in which context we are + * doing reconfiguration. + */ + for (p_ccb = p_lcb->ccb_queue.p_first_ccb; p_ccb; p_ccb = p_ccb->p_next_ccb) + if ((p_ccb->in_use) && (p_ccb->ecoc) && (p_ccb->reconfig_started)) { + p_ccb->p_lcb->pending_ecoc_reconfig_cfg = *p_cfg; + p_ccb->p_lcb->pending_ecoc_reconfig_cnt = lcids.size(); + break; + } + + l2c_csm_execute(p_ccb, L2CEVT_L2CA_CREDIT_BASED_RECONFIG_REQ, p_cfg); + + return (true); +} + +/******************************************************************************* + * * Function L2CA_DisconnectReq * * Description Higher layers call this function to disconnect a channel. diff --git a/stack/l2cap/l2c_ble.cc b/stack/l2cap/l2c_ble.cc index 6b6cf6440..dbe055c09 100644 --- a/stack/l2cap/l2c_ble.cc +++ b/stack/l2cap/l2c_ble.cc @@ -484,6 +484,8 @@ void l2cble_process_sig_cmd(tL2C_LCB* p_lcb, uint8_t* p, uint16_t pkt_len) { tL2C_CCB *p_ccb = NULL, *temp_p_ccb = NULL; tL2C_RCB* p_rcb; uint16_t credit; + uint8_t num_of_channels; + p_pkt_end = p + pkt_len; if (p + 4 > p_pkt_end) { @@ -563,6 +565,341 @@ void l2cble_process_sig_cmd(tL2C_LCB* p_lcb, uint8_t* p, uint16_t pkt_len) { p += 2; break; + case L2CAP_CMD_CREDIT_BASED_CONN_REQ: { + if (p + 10 > p_pkt_end) { + LOG(ERROR) << "invalid L2CAP_CMD_CREDIT_BASED_CONN_REQ len"; + return; + } + + STREAM_TO_UINT16(con_info.psm, p); + STREAM_TO_UINT16(mtu, p); + STREAM_TO_UINT16(mps, p); + STREAM_TO_UINT16(initial_credit, p); + + /* Check how many channels remote side wants. */ + num_of_channels = (p_pkt_end - p) / sizeof(uint16_t); + + L2CAP_TRACE_DEBUG( + "Recv L2CAP_CMD_CREDIT_BASED_CONN_REQ with " + "mtu = %d, " + "mps = %d, " + "initial credit = %d" + "num_of_channels = %d", + mtu, mps, initial_credit, num_of_channels); + + if (p_lcb->pending_ecoc_connection_cids.size() > 0) { + L2CAP_TRACE_WARNING( + "L2CAP - L2CAP_CMD_CREDIT_BASED_CONN_REQ collision:"); + l2cu_reject_credit_based_conn_req(p_lcb, id, num_of_channels, + L2CAP_LE_RESULT_NO_RESOURCES); + return; + } + + /* Check PSM Support */ + p_rcb = l2cu_find_ble_rcb_by_psm(con_info.psm); + if (p_rcb == NULL) { + L2CAP_TRACE_WARNING("L2CAP - rcvd conn req for unknown PSM: 0x%04x", + con_info.psm); + l2cu_reject_credit_based_conn_req(p_lcb, id, num_of_channels, + L2CAP_LE_RESULT_NO_PSM); + return; + } + + if (!p_rcb->api.pL2CA_CreditBasedConnectInd_Cb) { + L2CAP_TRACE_WARNING( + "L2CAP - rcvd conn req for outgoing-only connection PSM: %d", + con_info.psm); + l2cu_reject_credit_based_conn_req(p_lcb, id, num_of_channels, + L2CAP_CONN_NO_PSM); + return; + } + + /* validate the parameters */ + if (mtu < L2CAP_CREDIT_BASED_MIN_MTU || + mps < L2CAP_CREDIT_BASED_MIN_MPS || mps > L2CAP_LE_MAX_MPS) { + L2CAP_TRACE_ERROR("L2CAP don't like the params"); + l2cu_reject_credit_based_conn_req(p_lcb, id, num_of_channels, + L2CAP_LE_RESULT_INVALID_PARAMETERS); + return; + } + + /* Clear previous list */ + p_lcb->pending_ecoc_allocated_cids.clear(); + + for (int i = 0; i < num_of_channels; i++) { + STREAM_TO_UINT16(rcid, p); + temp_p_ccb = l2cu_find_ccb_by_remote_cid(p_lcb, rcid); + if (temp_p_ccb) { + L2CAP_TRACE_WARNING( + "L2CAP - rcvd conn req for duplicated cid: 0x%04x", rcid); + p_lcb->pending_ecoc_connection_cids.push_back(0); + p_lcb->pending_l2cap_result = + L2CAP_LE_RESULT_SOURCE_CID_ALREADY_ALLOCATED; + } else { + /* Allocate a ccb for this.*/ + temp_p_ccb = l2cu_allocate_ccb(p_lcb, 0); + if (temp_p_ccb == NULL) { + L2CAP_TRACE_ERROR("L2CAP - unable to allocate CCB"); + p_lcb->pending_ecoc_connection_cids.push_back(0); + p_lcb->pending_l2cap_result = L2CAP_LE_RESULT_NO_RESOURCES; + continue; + } + + temp_p_ccb->ecoc = true; + temp_p_ccb->remote_id = id; + temp_p_ccb->p_rcb = p_rcb; + temp_p_ccb->remote_cid = rcid; + + temp_p_ccb->peer_conn_cfg.mtu = mtu; + temp_p_ccb->peer_conn_cfg.mps = mps; + temp_p_ccb->peer_conn_cfg.credits = initial_credit; + + temp_p_ccb->tx_mps = mps; + temp_p_ccb->ble_sdu = NULL; + temp_p_ccb->ble_sdu_length = 0; + temp_p_ccb->is_first_seg = true; + temp_p_ccb->peer_cfg.fcr.mode = L2CAP_FCR_LE_COC_MODE; + + /* This list will be used to prepare response */ + p_lcb->pending_ecoc_connection_cids.push_back(temp_p_ccb->local_cid); + + /* This is used to notify user */ + p_lcb->pending_ecoc_allocated_cids.push_back(temp_p_ccb->local_cid); + + /*This is going to be our lead p_ccb for state machine */ + if (p_lcb->pending_ecoc_allocated_cids.size() == 1) { + p_ccb = temp_p_ccb; + p_lcb->pending_lead_cid = p_ccb->local_cid; + } + } + } + + if (p_lcb->pending_ecoc_allocated_cids.size() == 0) { + L2CAP_TRACE_ERROR("L2CAP - unable to allocate CCB"); + l2cu_reject_credit_based_conn_req(p_lcb, id, num_of_channels, + p_lcb->pending_l2cap_result); + return; + } + + l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CREDIT_BASED_CONNECT_REQ, NULL); + break; + } + case L2CAP_CMD_CREDIT_BASED_CONN_RES: + if (p + 2 > p_pkt_end) { + LOG(ERROR) << "invalid L2CAP_CMD_CREDIT_BASED_CONN_RES len"; + return; + } + + L2CAP_TRACE_DEBUG("Recv L2CAP_CMD_CREDIT_BASED_CONN_RES"); + /* For all channels, see whose identifier matches this id */ + for (temp_p_ccb = p_lcb->ccb_queue.p_first_ccb; temp_p_ccb; + temp_p_ccb = temp_p_ccb->p_next_ccb) { + if (temp_p_ccb->local_id == id) { + p_ccb = temp_p_ccb; + break; + } + } + + if (!p_ccb) { + L2CAP_TRACE_DEBUG(" Cannot find matching connection req"); + con_info.l2cap_result = L2CAP_LE_RESULT_INVALID_SOURCE_CID; + l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CONNECT_RSP_NEG, &con_info); + return; + } + + STREAM_TO_UINT16(mtu, p); + STREAM_TO_UINT16(mps, p); + STREAM_TO_UINT16(initial_credit, p); + STREAM_TO_UINT16(con_info.l2cap_result, p); + + /* When one of these result is sent back that means, + * all the channels has been rejected + */ + if (con_info.l2cap_result == L2CAP_LE_RESULT_NO_PSM || + con_info.l2cap_result == + L2CAP_LE_RESULT_INSUFFICIENT_AUTHENTICATION || + con_info.l2cap_result == L2CAP_LE_RESULT_INSUFFICIENT_ENCRYP || + con_info.l2cap_result == L2CAP_LE_RESULT_INSUFFICIENT_AUTHORIZATION || + con_info.l2cap_result == L2CAP_LE_RESULT_UNACCEPTABLE_PARAMETERS || + con_info.l2cap_result == L2CAP_LE_RESULT_INVALID_PARAMETERS) { + l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP_NEG, + &con_info); + return; + } + + /* validate the parameters */ + if (mtu < L2CAP_CREDIT_BASED_MIN_MTU || + mps < L2CAP_CREDIT_BASED_MIN_MPS || mps > L2CAP_LE_MAX_MPS) { + L2CAP_TRACE_ERROR("L2CAP - invalid params"); + con_info.l2cap_result = L2CAP_LE_RESULT_INVALID_PARAMETERS; + l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP_NEG, + &con_info); + break; + } + + /* At least some of the channels has been created and parameters are + * good*/ + num_of_channels = (p_pkt_end - p) / sizeof(uint16_t); + if (num_of_channels != p_lcb->pending_ecoc_connection_cids.size()) { + L2CAP_TRACE_ERROR( + "Incorrect response." + "expected num of channels = %d", + "received num of channels = %d", num_of_channels, + p_lcb->pending_ecoc_connection_cids.size()); + return; + } + + L2CAP_TRACE_DEBUG( + "mtu = %d, " + "mps = %d, " + "initial_credit = %d, " + "con_info.l2cap_result = %d" + "num_of_channels = %d", + mtu, mps, initial_credit, con_info.l2cap_result, num_of_channels); + + con_info.peer_mtu = mtu; + + for (uint16_t cid : p_lcb->pending_ecoc_connection_cids) { + temp_p_ccb = l2cu_find_ccb_by_cid(p_lcb, cid); + STREAM_TO_UINT16(temp_p_ccb->remote_cid, p); + + L2CAP_TRACE_DEBUG( + "local cid = %d " + "remote cid = %d", + cid, temp_p_ccb->remote_cid); + + /* Check if peer accepted channel, if not release the one not + * created + */ + if (temp_p_ccb->remote_cid == 0) { + l2c_csm_execute(temp_p_ccb, L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP_NEG, + &con_info); + } else { + temp_p_ccb->tx_mps = mps; + temp_p_ccb->ble_sdu = NULL; + temp_p_ccb->ble_sdu_length = 0; + temp_p_ccb->is_first_seg = true; + temp_p_ccb->peer_cfg.fcr.mode = L2CAP_FCR_LE_COC_MODE; + temp_p_ccb->peer_conn_cfg.mtu = mtu; + temp_p_ccb->peer_conn_cfg.mps = mps; + temp_p_ccb->peer_conn_cfg.credits = initial_credit; + + l2c_csm_execute(temp_p_ccb, L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP, + &con_info); + } + } + + p_lcb->pending_ecoc_connection_cids.clear(); + + break; + case L2CAP_CMD_CREDIT_BASED_RECONFIG_REQ: { + if (p + 6 > p_pkt_end) { + l2cu_send_ble_reconfig_rsp(p_lcb, id, L2CAP_RECONFIG_UNACCAPTED_PARAM); + return; + } + + STREAM_TO_UINT16(mtu, p); + STREAM_TO_UINT16(mps, p); + + /* validate the parameters */ + if (mtu < L2CAP_CREDIT_BASED_MIN_MTU || + mps < L2CAP_CREDIT_BASED_MIN_MPS || mps > L2CAP_LE_MAX_MPS) { + L2CAP_TRACE_ERROR("L2CAP - invalid params"); + l2cu_send_ble_reconfig_rsp(p_lcb, id, L2CAP_RECONFIG_UNACCAPTED_PARAM); + return; + } + + /* Check how many channels remote side wants to reconfigure */ + num_of_channels = (p_pkt_end - p) / sizeof(uint16_t); + + L2CAP_TRACE_DEBUG( + "Recv L2CAP_CMD_CREDIT_BASED_RECONFIG_REQ with " + "mtu = %d, " + "mps = %d, " + "num_of_channels = %d", + mtu, mps, num_of_channels); + + uint8_t* p_tmp = p; + for (int i = 0; i < num_of_channels; i++) { + STREAM_TO_UINT16(rcid, p_tmp); + p_ccb = l2cu_find_ccb_by_remote_cid(p_lcb, rcid); + if (!p_ccb) { + L2CAP_TRACE_WARNING( + "L2CAP - rcvd config req for non existing cid: 0x%04x", rcid); + l2cu_send_ble_reconfig_rsp(p_lcb, id, L2CAP_RECONFIG_INVALID_DCID); + return; + } + + if (p_ccb->peer_conn_cfg.mtu > mtu) { + L2CAP_TRACE_WARNING( + "L2CAP - rcvd config req mtu reduction new mtu < mtu (%d < %d)", + mtu, p_ccb->peer_conn_cfg.mtu); + l2cu_send_ble_reconfig_rsp(p_lcb, id, + L2CAP_RECONFIG_REDUCTION_MTU_NO_ALLOWED); + return; + } + + if (p_ccb->peer_conn_cfg.mps > mps) { + L2CAP_TRACE_WARNING( + "L2CAP - rcvd config req mps reduction new mps < mps (%d < %d)", + mtu, p_ccb->peer_conn_cfg.mtu); + l2cu_send_ble_reconfig_rsp(p_lcb, id, + L2CAP_RECONFIG_REDUCTION_MPS_NO_ALLOWED); + return; + } + } + + for (int i = 0; i < num_of_channels; i++) { + STREAM_TO_UINT16(rcid, p); + + /* Store new values */ + p_ccb = l2cu_find_ccb_by_remote_cid(p_lcb, rcid); + p_ccb->peer_conn_cfg.mtu = mtu; + p_ccb->peer_conn_cfg.mps = mps; + p_ccb->tx_mps = mps; + + tL2CAP_LE_CFG_INFO le_cfg; + le_cfg.mps = mps; + le_cfg.mtu = mtu; + + l2c_csm_execute(p_ccb, L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_REQ, &le_cfg); + } + + l2cu_send_ble_reconfig_rsp(p_lcb, id, L2CAP_RECONFIG_SUCCEED); + + break; + } + + case L2CAP_CMD_CREDIT_BASED_RECONFIG_RES: { + uint16_t result; + STREAM_TO_UINT16(result, p); + + L2CAP_TRACE_DEBUG( + "Recv L2CAP_CMD_CREDIT_BASED_RECONFIG_RES for " + "result = 0x%04x", + result); + + p_lcb->pending_ecoc_reconfig_cfg.result = result; + + /* All channels which are in reconfiguration state are marked with + * reconfig_started flag. Find it and send response + */ + for (temp_p_ccb = p_lcb->ccb_queue.p_first_ccb; temp_p_ccb; + temp_p_ccb = temp_p_ccb->p_next_ccb) { + if ((temp_p_ccb->in_use) && (temp_p_ccb->reconfig_started)) { + l2c_csm_execute(temp_p_ccb, L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_RSP, + &p_lcb->pending_ecoc_reconfig_cfg); + + temp_p_ccb->reconfig_started = false; + if (result == L2CAP_CFG_OK) { + temp_p_ccb->local_conn_cfg = p_lcb->pending_ecoc_reconfig_cfg; + } + } + } + + break; + } + case L2CAP_CMD_BLE_CREDIT_BASED_CONN_REQ: if (p + 10 > p_pkt_end) { android_errorWriteLog(0x534e4554, "80261585"); @@ -1075,7 +1412,11 @@ void l2cble_credit_based_conn_req(tL2C_CCB* p_ccb) { return; } - l2cu_send_peer_ble_credit_based_conn_req(p_ccb); + if (p_ccb->ecoc) { + l2cu_send_peer_credit_based_conn_req(p_ccb); + } else { + l2cu_send_peer_ble_credit_based_conn_req(p_ccb); + } return; } diff --git a/stack/l2cap/l2c_csm.cc b/stack/l2cap/l2c_csm.cc index 5312a9c72..881b8b93e 100644 --- a/stack/l2cap/l2c_csm.cc +++ b/stack/l2cap/l2c_csm.cc @@ -214,7 +214,8 @@ static void l2c_csm_closed(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { } break; - case L2CEVT_L2CA_CONNECT_REQ: /* API connect request */ + case L2CEVT_L2CA_CREDIT_BASED_CONNECT_REQ: /* API connect request */ + case L2CEVT_L2CA_CONNECT_REQ: if (p_ccb->p_lcb->transport == BT_TRANSPORT_LE) { p_ccb->chnl_state = CST_ORIG_W4_SEC_COMP; l2ble_sec_access_req(p_ccb->p_lcb->remote_bd_addr, p_ccb->p_rcb->psm, @@ -263,7 +264,8 @@ static void l2c_csm_closed(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { (*p_ccb->p_rcb->api.pL2CA_Error_Cb)(local_cid, L2CAP_CONN_OTHER_ERROR); break; - case L2CEVT_L2CAP_CONNECT_REQ: /* Peer connect request */ + case L2CEVT_L2CAP_CREDIT_BASED_CONNECT_REQ: /* Peer connect request */ + case L2CEVT_L2CAP_CONNECT_REQ: /* stop link timer to avoid race condition between A2MP, Security, and * L2CAP */ alarm_cancel(p_ccb->p_lcb->l2c_lcb_timer); @@ -275,6 +277,9 @@ static void l2c_csm_closed(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { &l2c_link_sec_comp2, p_ccb); switch (result) { + case L2CAP_LE_RESULT_INSUFFICIENT_AUTHORIZATION: + case L2CAP_LE_RESULT_UNACCEPTABLE_PARAMETERS: + case L2CAP_LE_RESULT_INVALID_PARAMETERS: case L2CAP_LE_RESULT_INSUFFICIENT_AUTHENTICATION: case L2CAP_LE_RESULT_INSUFFICIENT_ENCRYP_KEY_SIZE: case L2CAP_LE_RESULT_INSUFFICIENT_ENCRYP: @@ -457,9 +462,20 @@ static void l2c_csm_term_w4_sec_comp(tL2C_CCB* p_ccb, uint16_t event, if (p_ccb->p_lcb->transport != BT_TRANSPORT_LE) { l2c_csm_send_config_req(p_ccb); } else { - L2CAP_TRACE_API("L2CAP - Calling Connect_Ind_Cb(), CID: 0x%04x", - p_ccb->local_cid); - l2c_csm_indicate_connection_open(p_ccb); + if (p_ccb->ecoc) { + L2CAP_TRACE_API( + "L2CAP - Calling CreditBasedConnect_Ind_Cb(), num of cids: %d", + p_ccb->p_lcb->pending_ecoc_connection_cids.size()); + + (*p_ccb->p_rcb->api.pL2CA_CreditBasedConnectInd_Cb)( + p_ccb->p_lcb->remote_bd_addr, + p_ccb->p_lcb->pending_ecoc_connection_cids, p_ccb->p_rcb->psm, + p_ccb->peer_cfg.mtu, p_ccb->remote_id); + } else { + L2CAP_TRACE_API("L2CAP - Calling Connect_Ind_Cb(), CID: 0x%04x", + p_ccb->local_cid); + l2c_csm_indicate_connection_open(p_ccb); + } } } else { /* @@ -541,7 +557,10 @@ static void l2c_csm_w4_l2cap_connect_rsp(tL2C_CCB* p_ccb, uint16_t event, tL2C_CONN_INFO* p_ci = (tL2C_CONN_INFO*)p_data; tL2CA_DISCONNECT_IND_CB* disconnect_ind = p_ccb->p_rcb->api.pL2CA_DisconnectInd_Cb; + tL2CA_CREDIT_BASED_CONNECT_CFM_CB* credit_based_connect_cfm = + p_ccb->p_rcb->api.pL2CA_CreditBasedConnectCfm_Cb; uint16_t local_cid = p_ccb->local_cid; + tL2C_LCB* p_lcb = p_ccb->p_lcb; L2CAP_TRACE_EVENT("L2CAP - LCID: 0x%04x st: W4_L2CAP_CON_RSP evt: %s", p_ccb->local_cid, l2c_csm_get_event_name(event)); @@ -589,6 +608,28 @@ static void l2c_csm_w4_l2cap_connect_rsp(tL2C_CCB* p_ccb, uint16_t event, l2c_ccb_timer_timeout, p_ccb); break; + case L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP: + alarm_cancel(p_ccb->l2c_ccb_timer); + p_ccb->chnl_state = CST_OPEN; + L2CAP_TRACE_API( + "L2CAP - Calling credit_based_connect_cfm()," + "cid %d, result 0x%04x", + p_ccb->local_cid, L2CAP_CONN_OK); + + (*credit_based_connect_cfm)(p_lcb->remote_bd_addr, p_ccb->local_cid, + p_ci->peer_mtu, L2CAP_CONN_OK); + break; + + case L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP_NEG: + L2CAP_TRACE_API( + "L2CAP - Calling pL2CA_Error_Cb()," + "cid %d, result 0x%04x", + local_cid, p_ci->l2cap_result); + (*p_ccb->p_rcb->api.pL2CA_Error_Cb)(local_cid, p_ci->l2cap_result); + + l2cu_release_ccb(p_ccb); + break; + case L2CEVT_L2CAP_CONNECT_RSP_NEG: /* Peer rejected connection */ LOG(WARNING) << __func__ << ": L2CAP connection rejected, lcid=" << loghex(p_ccb->local_cid) @@ -598,10 +639,23 @@ static void l2c_csm_w4_l2cap_connect_rsp(tL2C_CCB* p_ccb, uint16_t event, break; case L2CEVT_TIMEOUT: - LOG(WARNING) << __func__ << ": L2CAP connection timeout, lcid=" - << loghex(p_ccb->local_cid); - l2cu_release_ccb(p_ccb); - (*p_ccb->p_rcb->api.pL2CA_Error_Cb)(local_cid, L2CAP_CONN_OTHER_ERROR); + LOG(WARNING) << __func__ << ": L2CAP connection timeout"; + + if (p_ccb->ecoc) { + for (uint16_t cid : p_lcb->pending_ecoc_connection_cids) { + tL2C_CCB* temp_p_ccb = l2cu_find_ccb_by_cid(p_lcb, cid); + LOG(WARNING) << __func__ << ": lcid= " << loghex(cid); + (*p_ccb->p_rcb->api.pL2CA_Error_Cb)(p_ccb->local_cid, + L2CAP_CONN_TIMEOUT); + l2cu_release_ccb(temp_p_ccb); + } + p_lcb->pending_ecoc_connection_cids.clear(); + + } else { + LOG(WARNING) << __func__ << ": lcid= " << loghex(p_ccb->local_cid); + l2cu_release_ccb(p_ccb); + (*p_ccb->p_rcb->api.pL2CA_Error_Cb)(local_cid, L2CAP_CONN_OTHER_ERROR); + } break; case L2CEVT_L2CA_DISCONNECT_REQ: /* Upper wants to disconnect */ @@ -666,6 +720,28 @@ static void l2c_csm_w4_l2ca_connect_rsp(tL2C_CCB* p_ccb, uint16_t event, (*disconnect_ind)(local_cid, false); break; + case L2CEVT_L2CA_CREDIT_BASED_CONNECT_RSP: + p_ci = (tL2C_CONN_INFO*)p_data; + if (p_ccb->p_lcb && p_ccb->p_lcb->transport != BT_TRANSPORT_LE) { + L2CAP_TRACE_WARNING("LE link doesn't exist"); + return; + } + l2cu_send_peer_credit_based_conn_res(p_ccb, p_ci->lcids, + p_ci->l2cap_result); + alarm_cancel(p_ccb->l2c_ccb_timer); + + for (uint16_t cid : p_ccb->p_lcb->pending_ecoc_connection_cids) { + tL2C_CCB* temp_p_ccb = l2cu_find_ccb_by_cid(p_ccb->p_lcb, cid); + auto it = std::find(p_ci->lcids.begin(), p_ci->lcids.end(), cid); + if (it != p_ci->lcids.end()) { + temp_p_ccb->chnl_state = CST_OPEN; + } else { + l2cu_release_ccb(temp_p_ccb); + } + } + p_ccb->p_lcb->pending_ecoc_connection_cids.clear(); + + break; case L2CEVT_L2CA_CONNECT_RSP: p_ci = (tL2C_CONN_INFO*)p_data; if (p_ccb->p_lcb->transport == BT_TRANSPORT_LE) { @@ -696,6 +772,19 @@ static void l2c_csm_w4_l2ca_connect_rsp(tL2C_CCB* p_ccb, uint16_t event, } break; + case L2CEVT_L2CA_CREDIT_BASED_CONNECT_RSP_NEG: + p_ci = (tL2C_CONN_INFO*)p_data; + if (p_ccb->p_lcb && p_ccb->p_lcb->transport == BT_TRANSPORT_LE) { + l2cu_send_peer_credit_based_conn_res(p_ccb, p_ci->lcids, + p_ci->l2cap_result); + } + alarm_cancel(p_ccb->l2c_ccb_timer); + for (uint16_t cid : p_ccb->p_lcb->pending_ecoc_connection_cids) { + tL2C_CCB* temp_p_ccb = l2cu_find_ccb_by_cid(p_ccb->p_lcb, cid); + l2cu_release_ccb(temp_p_ccb); + } + p_ccb->p_lcb->pending_ecoc_connection_cids.clear(); + break; case L2CEVT_L2CA_CONNECT_RSP_NEG: p_ci = (tL2C_CONN_INFO*)p_data; if (p_ccb->p_lcb->transport == BT_TRANSPORT_LE) @@ -756,6 +845,9 @@ static void l2c_csm_config(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { p_ccb->p_rcb->api.pL2CA_DisconnectInd_Cb; uint16_t local_cid = p_ccb->local_cid; uint8_t cfg_result; + tL2C_LCB* p_lcb = p_ccb->p_lcb; + tL2C_CCB* temp_p_ccb; + tL2CAP_LE_CFG_INFO* p_le_cfg = (tL2CAP_LE_CFG_INFO*)p_data; L2CAP_TRACE_EVENT("L2CAP - LCID: 0x%04x st: CONFIG evt: %s", p_ccb->local_cid, l2c_csm_get_event_name(event)); @@ -769,8 +861,17 @@ static void l2c_csm_config(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { (*disconnect_ind)(local_cid, false); break; - case L2CEVT_L2CAP_CONFIG_REQ: /* Peer config request */ + case L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_REQ: + /* For ecoc reconfig is handled below in l2c_ble. In case of success + * let us notify upper layer about the reconfig + */ + L2CAP_TRACE_API("L2CAP - Calling LeReconfigCompleted_Cb(), CID: 0x%04x", + p_ccb->local_cid); + (*p_ccb->p_rcb->api.pL2CA_CreditBasedReconfigCompleted_Cb)( + p_lcb->remote_bd_addr, p_ccb->local_cid, false, p_le_cfg); + break; + case L2CEVT_L2CAP_CONFIG_REQ: /* Peer config request */ cfg_result = l2cu_process_peer_cfg_req(p_ccb, p_cfg); if (cfg_result == L2CAP_PEER_CFG_OK) { L2CAP_TRACE_EVENT( @@ -799,6 +900,19 @@ static void l2c_csm_config(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { } break; + case L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_RSP: + p_ccb->config_done |= OB_CFG_DONE; + p_ccb->config_done |= RECONFIG_FLAG; + p_ccb->chnl_state = CST_OPEN; + alarm_cancel(p_ccb->l2c_ccb_timer); + + L2CAP_TRACE_API("L2CAP - Calling Config_Rsp_Cb(), CID: 0x%04x", + p_ccb->local_cid); + + p_ccb->p_rcb->api.pL2CA_CreditBasedReconfigCompleted_Cb( + p_lcb->remote_bd_addr, p_ccb->local_cid, true, p_le_cfg); + + break; case L2CEVT_L2CAP_CONFIG_RSP: /* Peer config response */ l2cu_process_peer_cfg_rsp(p_ccb, p_cfg); @@ -884,6 +998,11 @@ static void l2c_csm_config(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { l2c_csm_send_disconnect_rsp(p_ccb); break; + case L2CEVT_L2CA_CREDIT_BASED_RECONFIG_REQ: + l2cu_send_credit_based_reconfig_req(p_ccb, (tL2CAP_LE_CFG_INFO*)p_data); + alarm_set_on_mloop(p_ccb->l2c_ccb_timer, L2CAP_CHNL_CFG_TIMEOUT_MS, + l2c_ccb_timer_timeout, p_ccb); + break; case L2CEVT_L2CA_CONFIG_REQ: /* Upper layer config req */ l2cu_process_our_cfg_req(p_ccb, p_cfg); l2cu_send_peer_config_req(p_ccb, p_cfg); @@ -968,6 +1087,21 @@ static void l2c_csm_config(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { break; case L2CEVT_TIMEOUT: + if (p_ccb->ecoc) { + for (temp_p_ccb = p_lcb->ccb_queue.p_first_ccb; temp_p_ccb; + temp_p_ccb = temp_p_ccb->p_next_ccb) { + if ((temp_p_ccb->in_use) && (temp_p_ccb->reconfig_started)) { + (*temp_p_ccb->p_rcb->api.pL2CA_DisconnectInd_Cb)( + temp_p_ccb->local_cid, false); + l2cu_release_ccb(temp_p_ccb); + } + } + + btsnd_hcic_disconnect(p_ccb->p_lcb->Handle(), + HCI_ERR_CONN_CAUSE_LOCAL_HOST); + return; + } + l2cu_send_peer_disc_req(p_ccb); L2CAP_TRACE_API( "L2CAP - Calling Disconnect_Ind_Cb(), CID: 0x%04x No Conf Needed", @@ -995,6 +1129,7 @@ static void l2c_csm_open(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { uint8_t tempcfgdone; uint8_t cfg_result; uint16_t credit = 0; + tL2CAP_LE_CFG_INFO* p_le_cfg = (tL2CAP_LE_CFG_INFO*)p_data; L2CAP_TRACE_EVENT("L2CAP - LCID: 0x%04x st: OPEN evt: %s", p_ccb->local_cid, l2c_csm_get_event_name(event)); @@ -1009,6 +1144,17 @@ static void l2c_csm_open(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { (*p_ccb->p_rcb->api.pL2CA_DisconnectInd_Cb)(local_cid, false); break; + case L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_REQ: + /* For ecoc reconfig is handled below in l2c_ble. In case of success + * let us notify upper layer about the reconfig + */ + L2CAP_TRACE_API("L2CAP - Calling LeReconfigCompleted_Cb(), CID: 0x%04x", + p_ccb->local_cid); + + (*p_ccb->p_rcb->api.pL2CA_CreditBasedReconfigCompleted_Cb)( + p_ccb->p_lcb->remote_bd_addr, p_ccb->local_cid, false, p_le_cfg); + break; + case L2CEVT_L2CAP_CONFIG_REQ: /* Peer config request */ p_cfg = (tL2CAP_CFG_INFO*)p_data; @@ -1097,6 +1243,16 @@ static void l2c_csm_open(tL2C_CCB* p_ccb, uint16_t event, void* p_data) { l2c_link_check_send_pkts(p_ccb->p_lcb, 0, NULL); break; + case L2CEVT_L2CA_CREDIT_BASED_RECONFIG_REQ: + p_ccb->chnl_state = CST_CONFIG; + p_ccb->config_done &= ~OB_CFG_DONE; + + l2cu_send_credit_based_reconfig_req(p_ccb, (tL2CAP_LE_CFG_INFO*)p_data); + + alarm_set_on_mloop(p_ccb->l2c_ccb_timer, L2CAP_CHNL_CFG_TIMEOUT_MS, + l2c_ccb_timer_timeout, p_ccb); + break; + case L2CEVT_L2CA_CONFIG_REQ: /* Upper layer config req */ L2CAP_TRACE_ERROR( "Dropping L2CAP re-config request because there is no usage and " @@ -1301,11 +1457,31 @@ static const char* l2c_csm_get_event_name(uint16_t event) { case L2CEVT_ACK_TIMEOUT: return ("L2CEVT_ACK_TIMEOUT"); case L2CEVT_L2CA_SEND_FLOW_CONTROL_CREDIT: /* Upper layer send credit packet - */ + */ return ("SEND_FLOW_CONTROL_CREDIT"); + case L2CEVT_L2CA_CREDIT_BASED_CONNECT_REQ: /* Upper layer credit based + connect request */ + return ("SEND_CREDIT_BASED_CONNECT_REQ"); + case L2CEVT_L2CA_CREDIT_BASED_RECONFIG_REQ: /* Upper layer credit based + reconfig request */ + return ("SEND_CREDIT_BASED_RECONFIG_REQ"); case L2CEVT_L2CAP_RECV_FLOW_CONTROL_CREDIT: /* Peer send credit packet */ return ("RECV_FLOW_CONTROL_CREDIT"); - + case L2CEVT_L2CAP_CREDIT_BASED_CONNECT_REQ: /* Peer send credit based + connect request */ + return ("RECV_CREDIT_BASED_CONNECT_REQ"); + case L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP: /* Peer send credit based + connect response */ + return ("RECV_CREDIT_BASED_CONNECT_RSP"); + case L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP_NEG: /* Peer send reject credit + based connect response */ + return ("RECV_CREDIT_BASED_CONNECT_RSP_NEG"); + case L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_REQ: /* Peer send credit based + reconfig request */ + return ("RECV_CREDIT_BASED_RECONFIG_REQ"); + case L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_RSP: /* Peer send credit based + reconfig response */ + return ("RECV_CREDIT_BASED_RECONFIG_RSP"); default: return ("???? UNKNOWN EVENT"); } diff --git a/stack/l2cap/l2c_int.h b/stack/l2cap/l2c_int.h index 523456025..8511412f0 100644 --- a/stack/l2cap/l2c_int.h +++ b/stack/l2cap/l2c_int.h @@ -41,6 +41,8 @@ #define L2CAP_MIN_MTU 48 /* Minimum acceptable MTU is 48 bytes */ +constexpr uint16_t L2CAP_CREDIT_BASED_MIN_MTU = 64; +constexpr uint16_t L2CAP_CREDIT_BASED_MIN_MPS = 64; #define L2CAP_NO_IDLE_TIMEOUT 0xFFFF /* @@ -136,7 +138,25 @@ typedef enum { #define L2CEVT_L2CA_SEND_FLOW_CONTROL_CREDIT \ 35 /* Upper layer credit packet \ */ -#define L2CEVT_L2CAP_RECV_FLOW_CONTROL_CREDIT 36 /* Peer credit packet */ +/* Peer credit based connection */ +#define L2CEVT_L2CAP_RECV_FLOW_CONTROL_CREDIT 36 /* credit packet */ +#define L2CEVT_L2CAP_CREDIT_BASED_CONNECT_REQ \ + 37 /* credit based connection request */ +#define L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP \ + 38 /* accepted credit based connection */ +#define L2CEVT_L2CAP_CREDIT_BASED_CONNECT_RSP_NEG \ + 39 /* rejected credit based connection */ +#define L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_REQ \ + 40 /* credit based reconfig request*/ +#define L2CEVT_L2CAP_CREDIT_BASED_RECONFIG_RSP \ + 41 /* credit based reconfig response */ + +/* Upper layer credit based connection */ +#define L2CEVT_L2CA_CREDIT_BASED_CONNECT_REQ 42 /* connect request */ +#define L2CEVT_L2CA_CREDIT_BASED_CONNECT_RSP 43 /* connect response */ +#define L2CEVT_L2CA_CREDIT_BASED_CONNECT_RSP_NEG \ + 44 /* connect response (failed)*/ +#define L2CEVT_L2CA_CREDIT_BASED_RECONFIG_REQ 45 /* reconfig request */ /* Constants for LE Dynamic PSM values */ #define LE_DYNAMIC_PSM_START 0x0080 @@ -284,6 +304,10 @@ typedef struct t_l2c_ccb { /* Number of LE frames that the remote can send to us (credit count in * remote). Valid only for LE CoC */ uint16_t remote_credit_count; + + /* used to indicate that ECOC is used */ + bool ecoc; + bool reconfig_started; } tL2C_CCB; /*********************************************************************** @@ -432,6 +456,18 @@ typedef struct t_l2c_linkcb { tL2C_RR_SERV rr_serv[L2CAP_NUM_CHNL_PRIORITY]; uint8_t rr_pri; /* current serving priority group */ + /* Pending ECOC reconfiguration data */ + tL2CAP_LE_CFG_INFO pending_ecoc_reconfig_cfg; + uint8_t pending_ecoc_reconfig_cnt; + + /* This is to keep list of local cids use in the + * credit based connection response. + */ + std::vector pending_ecoc_connection_cids; + /* List of allocated cids, sent to user for acceptance */ + std::vector pending_ecoc_allocated_cids; + uint16_t pending_lead_cid; + uint16_t pending_l2cap_result; } tL2C_LCB; /* Define the L2CAP control structure @@ -519,6 +555,8 @@ typedef struct { uint16_t l2cap_result; /* L2CAP result */ uint16_t l2cap_status; /* L2CAP status */ uint16_t remote_cid; /* Remote CID */ + std::vector lcids; /* Used when credit based is used*/ + uint16_t peer_mtu; /* Peer MTU */ } tL2C_CONN_INFO; typedef void(tL2C_FCR_MGMT_EVT_HDLR)(uint8_t, tL2C_CCB*); @@ -599,11 +637,24 @@ extern void l2cu_send_peer_ble_par_rsp(tL2C_LCB* p_lcb, uint16_t reason, uint8_t rem_id); extern void l2cu_reject_ble_connection(tL2C_CCB* p_ccb, uint8_t rem_id, uint16_t result); +extern void l2cu_reject_credit_based_conn_req(tL2C_LCB* p_lcb, uint8_t rem_id, + uint8_t num_of_channels, + uint16_t result); extern void l2cu_reject_ble_coc_connection(tL2C_LCB* p_lcb, uint8_t rem_id, uint16_t result); extern void l2cu_send_peer_ble_credit_based_conn_res(tL2C_CCB* p_ccb, uint16_t result); +extern void l2cu_send_peer_credit_based_conn_res( + tL2C_CCB* p_ccb, std::vector& accepted_lcids, uint16_t result); + extern void l2cu_send_peer_ble_credit_based_conn_req(tL2C_CCB* p_ccb); +extern void l2cu_send_peer_credit_based_conn_req(tL2C_CCB* p_ccb); + +extern void l2cu_send_ble_reconfig_rsp(tL2C_LCB* p_lcb, uint8_t rem_id, + uint16_t result); +extern void l2cu_send_credit_based_reconfig_req(tL2C_CCB* p_ccb, + tL2CAP_LE_CFG_INFO* p_data); + extern void l2cu_send_peer_ble_flow_control_credit(tL2C_CCB* p_ccb, uint16_t credit_value); extern void l2cu_send_peer_ble_credit_based_disconn_req(tL2C_CCB* p_ccb); diff --git a/stack/l2cap/l2c_main.cc b/stack/l2cap/l2c_main.cc index ad07c3e89..074c57fd1 100644 --- a/stack/l2cap/l2c_main.cc +++ b/stack/l2cap/l2c_main.cc @@ -770,12 +770,14 @@ static void process_l2cap_cmd(tL2C_LCB* p_lcb, uint8_t* p, uint16_t pkt_len) { l2cu_process_fixed_chnl_resp(p_lcb); } - tL2C_CONN_INFO ci; - ci.status = HCI_SUCCESS; - ci.bd_addr = p_lcb->remote_bd_addr; - for (tL2C_CCB* p_ccb = p_lcb->ccb_queue.p_first_ccb; p_ccb; - p_ccb = p_ccb->p_next_ccb) { - l2c_csm_execute(p_ccb, L2CEVT_L2CAP_INFO_RSP, &ci); + { + tL2C_CONN_INFO ci; + ci.status = HCI_SUCCESS; + ci.bd_addr = p_lcb->remote_bd_addr; + for (tL2C_CCB* p_ccb = p_lcb->ccb_queue.p_first_ccb; p_ccb; + p_ccb = p_ccb->p_next_ccb) { + l2c_csm_execute(p_ccb, L2CEVT_L2CAP_INFO_RSP, &ci); + } } break; diff --git a/stack/l2cap/l2c_utils.cc b/stack/l2cap/l2c_utils.cc index cdb3ecd91..2aec262a2 100644 --- a/stack/l2cap/l2c_utils.cc +++ b/stack/l2cap/l2c_utils.cc @@ -491,6 +491,59 @@ void l2cu_reject_connection(tL2C_LCB* p_lcb, uint16_t remote_cid, /******************************************************************************* * + * Function l2cu_send_credit_based_reconfig_req + * + * Description Build and send an L2CAP "recoonfiguration request" message + * to the peer. + * + * Returns void + * + ******************************************************************************/ +void l2cu_send_credit_based_reconfig_req(tL2C_CCB* p_ccb, + tL2CAP_LE_CFG_INFO* p_cfg) { + BT_HDR* p_buf; + uint16_t cmd_len; + uint8_t* p; + tL2C_LCB* p_lcb = p_ccb->p_lcb; + tL2C_CCB* p_ccb_temp; + + cmd_len = L2CAP_CMD_CREDIT_BASED_RECONFIG_REQ_MIN_LEN + + sizeof(uint16_t) * p_lcb->pending_ecoc_reconfig_cnt; + + /* Create an identifier for this packet */ + p_lcb->signal_id++; + l2cu_adj_id(p_lcb); + + p_ccb->local_id = p_lcb->signal_id; + + p_buf = l2cu_build_header(p_lcb, cmd_len, L2CAP_CMD_CREDIT_BASED_RECONFIG_REQ, + p_lcb->signal_id); + if (p_buf == NULL) { + L2CAP_TRACE_WARNING("l2cu_send_reconfig_req - no buffer"); + return; + } + + p = (uint8_t*)(p_buf + 1) + L2CAP_SEND_CMD_OFFSET + HCI_DATA_PREAMBLE_SIZE + + L2CAP_PKT_OVERHEAD + L2CAP_CMD_OVERHEAD; + + L2CAP_TRACE_DEBUG("l2cu_send_reconfig_req number of cids: %d mtu:%d mps:%d", + p_lcb->pending_ecoc_reconfig_cnt, p_cfg->mtu, p_cfg->mps); + + UINT16_TO_STREAM(p, p_cfg->mtu); + UINT16_TO_STREAM(p, p_cfg->mps); + + for (p_ccb_temp = p_lcb->ccb_queue.p_first_ccb; p_ccb_temp; + p_ccb_temp = p_ccb_temp->p_next_ccb) { + if ((p_ccb_temp->in_use) && (p_ccb_temp->ecoc) && + (p_ccb_temp->reconfig_started)) + UINT16_TO_STREAM(p, p_ccb_temp->local_cid); + } + + l2c_link_check_send_pkts(p_lcb, 0, p_buf); +} + +/******************************************************************************* + * * Function l2cu_send_peer_config_req * * Description Build and send an L2CAP "configuration request" message @@ -2656,6 +2709,68 @@ void l2cu_send_peer_ble_credit_based_conn_req(tL2C_CCB* p_ccb) { /******************************************************************************* * + * Function l2cu_send_peer_credit_based_conn_req + * + * Description Build and send a BLE packet to establish enhanced connection + * oriented L2CAP channel. + * + * Returns void + * + ******************************************************************************/ +void l2cu_send_peer_credit_based_conn_req(tL2C_CCB* p_ccb) { + BT_HDR* p_buf; + uint8_t* p; + tL2C_LCB* p_lcb = NULL; + uint16_t mtu; + uint16_t mps; + uint16_t initial_credit; + + if (!p_ccb) return; + + p_lcb = p_ccb->p_lcb; + + /* Create an identifier for this packet */ + p_ccb->p_lcb->signal_id++; + l2cu_adj_id(p_ccb->p_lcb); + + p_ccb->local_id = p_lcb->signal_id; + + p_buf = l2cu_build_header(p_lcb, + L2CAP_CMD_CREDIT_BASED_CONN_REQ_MIN_LEN + + 2 * p_lcb->pending_ecoc_connection_cids.size(), + L2CAP_CMD_CREDIT_BASED_CONN_REQ, p_ccb->local_id); + if (p_buf == NULL) { + L2CAP_TRACE_WARNING("%s - no buffer", __func__); + return; + } + + p = (uint8_t*)(p_buf + 1) + L2CAP_SEND_CMD_OFFSET + HCI_DATA_PREAMBLE_SIZE + + L2CAP_PKT_OVERHEAD + L2CAP_CMD_OVERHEAD; + + mtu = p_ccb->local_conn_cfg.mtu; + mps = p_ccb->local_conn_cfg.mps; + initial_credit = p_ccb->local_conn_cfg.credits; + + L2CAP_TRACE_DEBUG( + "%s PSM:0x%04x mtu:%d mps:%d initial_credit:%d, cids_cnt %d", __func__, + p_ccb->p_rcb->real_psm, mtu, mps, initial_credit, + p_lcb->pending_ecoc_connection_cids.size()); + + UINT16_TO_STREAM(p, p_ccb->p_rcb->real_psm); + UINT16_TO_STREAM(p, mtu); + UINT16_TO_STREAM(p, mps); + UINT16_TO_STREAM(p, initial_credit); + + for (uint16_t cid : p_lcb->pending_ecoc_connection_cids) { + L2CAP_TRACE_DEBUG("\n\t cid: ", cid); + UINT16_TO_STREAM(p, cid); + } + + l2c_link_check_send_pkts(p_lcb, 0, p_buf); +} + +/******************************************************************************* + * * Function l2cu_reject_ble_coc_connection * * Description Build and send an L2CAP "Credit based connection res" @@ -2691,6 +2806,112 @@ void l2cu_reject_ble_coc_connection(tL2C_LCB* p_lcb, uint8_t rem_id, /******************************************************************************* * + * Function l2cu_reject_credit_based_connection_req + * + * Description Build and send an L2CAP "credit based connection + *res" message to the peer. This function is called for non-success cases. + * + * Returns void + * + ******************************************************************************/ +void l2cu_reject_credit_based_conn_req(tL2C_LCB* p_lcb, uint8_t rem_id, + uint8_t num_of_channels, + uint16_t result) { + BT_HDR* p_buf; + uint8_t* p; + uint8_t rsp_len = L2CAP_CMD_CREDIT_BASED_CONN_RES_MIN_LEN + + sizeof(uint16_t) * num_of_channels; + + p_buf = l2cu_build_header(p_lcb, rsp_len, L2CAP_CMD_CREDIT_BASED_CONN_RES, + rem_id); + if (p_buf == NULL) { + L2CAP_TRACE_WARNING("l2cu_reject_credit_based_conn_req - no buffer"); + return; + } + + p = (uint8_t*)(p_buf + 1) + L2CAP_SEND_CMD_OFFSET + HCI_DATA_PREAMBLE_SIZE + + L2CAP_PKT_OVERHEAD + L2CAP_CMD_OVERHEAD; + + memset(p, 0, rsp_len); + UINT16_TO_STREAM(p, L2CAP_CREDIT_BASED_MIN_MTU); /* dummy MTU to satisy PTS */ + UINT16_TO_STREAM(p, L2CAP_CREDIT_BASED_MIN_MPS); /* dummy MPS to satisy PTS*/ + UINT16_TO_STREAM(p, 1); /* dummy initial credit to satisy PTS */ + UINT16_TO_STREAM(p, result); + + l2c_link_check_send_pkts(p_lcb, 0, p_buf); +} + +/******************************************************************************* + * + * Function l2cu_send_peer_credit_based_conn_res + * + * Description Build and send an L2CAP "Credit based connection res" + * message to the peer. This function is called in case of + * success. + * + * Returns void + * + ******************************************************************************/ +void l2cu_send_peer_credit_based_conn_res(tL2C_CCB* p_ccb, + std::vector& accepted_cids, + uint16_t result) { + BT_HDR* p_buf; + uint8_t* p; + + L2CAP_TRACE_DEBUG("%s", __func__); + uint8_t rsp_len = + L2CAP_CMD_CREDIT_BASED_CONN_RES_MIN_LEN + + p_ccb->p_lcb->pending_ecoc_connection_cids.size() * sizeof(uint16_t); + + p_buf = l2cu_build_header(p_ccb->p_lcb, rsp_len, + L2CAP_CMD_CREDIT_BASED_CONN_RES, p_ccb->remote_id); + if (p_buf == NULL) { + L2CAP_TRACE_WARNING("%s - no buffer", __func__); + return; + } + + p = (uint8_t*)(p_buf + 1) + L2CAP_SEND_CMD_OFFSET + HCI_DATA_PREAMBLE_SIZE + + L2CAP_PKT_OVERHEAD + L2CAP_CMD_OVERHEAD; + + memset(p, 0, rsp_len); + UINT16_TO_STREAM(p, p_ccb->local_conn_cfg.mtu); /* MTU */ + UINT16_TO_STREAM(p, p_ccb->local_conn_cfg.mps); /* MPS */ + UINT16_TO_STREAM(p, p_ccb->local_conn_cfg.credits); /* initial credit */ + + if (result == L2CAP_CONN_OK) { + /* In case of success, we need to check if stack + * did not have previous result stored e.g. when there was no + * resources for allocation all the requrested channels, + * before user indication. + */ + result = p_ccb->p_lcb->pending_l2cap_result; + } + + UINT16_TO_STREAM(p, result); + + /* We need to keep order from the request. + * if this vector contais 0 it means channel has been rejected by + * the stack. + * If there is valid cid, we need to verify if it is accepted by upper layer. + */ + for (uint16_t cid : p_ccb->p_lcb->pending_ecoc_connection_cids) { + if (cid == 0) { + UINT16_TO_STREAM(p, 0); + continue; + } + auto it = std::find(accepted_cids.begin(), accepted_cids.end(), cid); + if (it != accepted_cids.end()) { + UINT16_TO_STREAM(p, cid); + } else { + UINT16_TO_STREAM(p, 0); + } + } + + l2c_link_check_send_pkts(p_ccb->p_lcb, 0, p_buf); +} + +/******************************************************************************* + * * Function l2cu_reject_ble_connection * * Description Build and send an L2CAP "Credit based connection res" @@ -2702,7 +2923,46 @@ void l2cu_reject_ble_coc_connection(tL2C_LCB* p_lcb, uint8_t rem_id, ******************************************************************************/ void l2cu_reject_ble_connection(tL2C_CCB* p_ccb, uint8_t rem_id, uint16_t result) { - l2cu_reject_ble_coc_connection(p_ccb->p_lcb, rem_id, result); + if (p_ccb->ecoc) + l2cu_reject_credit_based_conn_req( + p_ccb->p_lcb, rem_id, p_ccb->p_lcb->pending_ecoc_reconfig_cnt, result); + else + l2cu_reject_ble_coc_connection(p_ccb->p_lcb, rem_id, result); +} + +/******************************************************************************* + * + * Function l2cu_send_ble_reconfig_rsp + * + * Description Build and send an L2CAP "Credit based reconfig res" + * message to the peer. This function is called for non-success + * cases. + * + * Returns void + * + ******************************************************************************/ + +void l2cu_send_ble_reconfig_rsp(tL2C_LCB* p_lcb, uint8_t rem_id, + uint16_t result) { + BT_HDR* p_buf; + uint8_t* p; + + L2CAP_TRACE_DEBUG("l2cu_send_ble_reconfig_rsp result 0x04%x", result); + + p_buf = l2cu_build_header(p_lcb, L2CAP_CMD_CREDIT_BASED_RECONFIG_RES_LEN, + L2CAP_CMD_CREDIT_BASED_RECONFIG_RES, rem_id); + if (p_buf == NULL) { + L2CAP_TRACE_WARNING("l2cu_send_peer_ble_credit_based_conn_res - no buffer"); + return; + } + + p = (uint8_t*)(p_buf + 1) + L2CAP_SEND_CMD_OFFSET + HCI_DATA_PREAMBLE_SIZE + + L2CAP_PKT_OVERHEAD + L2CAP_CMD_OVERHEAD; + + memset(p, 0, L2CAP_CMD_CREDIT_BASED_RECONFIG_RES_LEN); + UINT16_TO_STREAM(p, result); + + l2c_link_check_send_pkts(p_lcb, 0, p_buf); } /*******************************************************************************