| /* |
| * Copyright 2017 The Chromium OS Authors. All rights reserved. |
| * Use of this source code is governed by a BSD-style license that can be |
| * found in the LICENSE file. |
| */ |
| //todo: check all lengths followed, all log levels correct |
| //todo: hci QoS is same as L2CAP QoS, so we can probably support L2CAP QoS via HCI QoS easily |
| |
| #include <stdlib.h> |
| #include <string.h> |
| #include <errno.h> |
| #include <sched.h> |
| #include "newblue-macros.h" |
| #include "multiNotif.h" |
| #include "workQueue.h" |
| #include "config.h" |
| #include "l2cap.h" |
| #include "timer.h" |
| #include "util.h" |
| #include "uniq.h" |
| #include "log.h" |
| #include "hci.h" |
| #include "mt.h" |
| |
| |
| /* constants from spec */ |
| #define L2C_FTR_FLOW_CTRL_MODE 0x00000001 |
| #define L2C_FTR_RETR_MODE 0x00000002 |
| #define L2C_FTR_BIDIR_QOS 0x00000004 |
| #define L2C_FTR_ENH_RETR_MODE 0x00000008 |
| #define L2C_FTR_STREAMING_MODE 0x00000010 |
| #define L2C_FTR_FCS_OPTION 0x00000020 |
| #define L2C_FTR_EXTD_FLOW_SPEC 0x00000040 |
| #define L2C_FTR_FIXED_CHS 0x00000080 |
| #define L2C_FTR_EXTD_WINDOW_SZ 0x00000100 |
| #define L2C_FTR_CONNLESS_DATA_RX 0x00000200 |
| |
| #define L2C_EXT_FTRS_SUPPORTED (L2C_FTR_FIXED_CHS) |
| #define L2C_FIXED_CHS_SUPPORTED 0x6 /* as mandated by the spec */ |
| #define L2C_MIN_CONNECTIONLESS_MTU 48 |
| #define L2C_MIN_SIG_MTU 48 |
| |
| #define L2C_RETR_NUM_BUFFERS 64 |
| |
| /* also, the first non-fixed channel */ |
| #define L2C_NUM_FIXED_CHANNELS 64 |
| |
| /* PSM length minimum as per spec */ |
| #define L2C_PSM_MIN_LEN 2 |
| |
| /* L2CAP channel defines as per spec */ |
| #define L2C_CH_SIGNALLING_EDR 0x0001 |
| #define L2C_CH_CONNECTIONLESS 0x0002 |
| #define L2C_CH_SIGNALLING_LE 0x0005 |
| |
| |
| #define L2C_MODE_BASIC 0 |
| #define L2C_MODE_RETR 1 |
| #define L2C_MODE_FLOW_CTRL 2 |
| #define L2C_MODE_STREAM 3 |
| #define L2C_MODE_ENH_RETR 4 |
| |
| #define L2C_FLG_S_FRM 0x0001 |
| #define L2C_FLG_R 0x0080 |
| #define L2C_FLG_F 0x0080 |
| #define L2C_MASK_SAR 0xC000 |
| #define L2C_SHFT_SAR 14 |
| #define L2C_MASK_S 0x000C |
| #define L2C_SHFT_S 2 |
| #define L2C_FLG_P 0x0010 |
| #define L2C_FLG_RSVD_S 0x0002 |
| #define L2C_FLG_MASK_REQ_SEQ 0x3F00 |
| #define L2C_FLG_SHFT_REQ_SEQ 8 |
| #define L2C_FLG_MASK_TX_SEQ 0x007E |
| #define L2C_FLG_SHFT_TX_SEQ 1 |
| |
| #define L2C_SAR_COMPLETE 0 |
| #define L2C_SAR_START 1 |
| #define L2C_SAR_END 2 |
| #define L2C_SAR_CONT 3 |
| |
| #define L2C_S_RR 0 |
| #define L2C_S_REJ 1 |
| #define L2C_S_RNR 2 |
| #define L2C_S_SREJ 3 |
| |
| |
| /* L2CAP signalling packet codes and respective formats */ |
| #define L2C_SIG_CMD_REJ 0x01 |
| struct l2cSigCmdRej { |
| uint16_t reason; |
| /* optional data */ |
| } __packed; |
| #define L2C_SIG_CMD_NOT_UNDERSTOD 0x0001 |
| /* no data */ |
| #define L2C_SIG_INVALID_CID 0x0003 |
| struct l2cSigInvalidCid { |
| uint16_t rxedDcid; |
| uint16_t rxedScid; |
| } __packed; |
| |
| #define L2C_SIG_CONN_REQ 0x02 |
| struct l2cSigConnReq { |
| psm_t psm; |
| uint16_t scid; |
| } __packed; |
| |
| #define L2C_SIG_CONN_RSP 0x03 |
| struct l2cSigConnRsp { |
| uint16_t dcid; |
| uint16_t scid; |
| uint16_t result; |
| uint16_t status; |
| } __packed; |
| #define L2C_SIG_CONN_RSP_OK 0x0000 |
| #define L2C_SIG_CONN_RSP_PEND 0x0001 |
| #define L2C_SIG_CONN_RSP_BAD_PSM 0x0002 |
| #define L2C_SIG_CONN_RSP_BAD_SEC 0x0003 |
| #define L2C_SIG_CONN_RSP_NO_RSRC 0x0004 |
| |
| #define L2C_SIG_CONN_PEND_UNKNOWN 0x0000 |
| #define L2C_SIG_CONN_PEND_AUTH 0x0001 |
| #define L2C_SIG_CONN_PEND_ATHORIZ 0x0002 |
| |
| #define L2C_SIG_CONF_REQ 0x04 |
| struct l2cSigConfReq { |
| uint16_t dcid; |
| uint16_t flags; |
| /* optional data */ |
| } __packed; |
| |
| #define L2C_CONF_MTU 0x01 /* payload: uint16 mtu */ |
| #define L2C_CONF_FLUSH_TIMEOUT 0x02 /* payload: uint16 timeout */ |
| #define L2C_CONF_QOS 0x03 /* 22 bytes of payload, beginning with uint8 SBZ, uint8 type (=1 for us) */ |
| #define L2C_CONF_RETR_AND_FLOW 0x04 /* 9 bytes of payload, beginnign with mode (=0 for us) */ |
| #define L2C_CONF_FCS 0x05 /* payload: uint8_t fcsType */ |
| #define L2C_CONF_EXT_FLOW 0x06 /* 16 bytes of payload, beginning with uint8 id(=1) and svc(=1) */ |
| |
| struct l2cConfQosOpts { |
| uint8_t flags; |
| uint8_t serviceType; |
| uint32_t tokenRate; |
| uint32_t tockenBucketSz; |
| uint32_t peakBandwidth; |
| uint32_t latency; |
| uint32_t delayVariation; |
| } __packed; |
| #define L2C_QOS_SVC_TYPE_BEST_EFFORT 0x01 |
| #define L2C_QOS_SVC_TYPE_GUARANTEED 0x02 |
| |
| struct l2cConfRetrOpts { |
| uint8_t mode; |
| uint8_t txWindowSize; |
| uint8_t maxTransmit; |
| uint16_t retrTimeout; |
| uint16_t monitorTimeout; |
| uint16_t maxPduSz; |
| } __packed; |
| #define L2C_RETR_MODE_BASIC 0x00 |
| |
| |
| |
| #define L2C_SIG_CONF_RSP 0x05 |
| struct l2cSigConfRsp { |
| uint16_t scid; |
| uint16_t flags; |
| uint16_t result; |
| /* optional data */ |
| } __packed; |
| #define L2C_SIG_CONF_RSP_OK 0x0000 |
| #define L2C_SIG_CONF_RSP_UNACCEPTABLE 0x0001 |
| #define L2C_SIG_CONF_RSP_REJECTED 0x0002 |
| #define L2C_SIG_CONF_RSP_UNKNOWN_OPTN 0x0003 |
| #define L2C_SIG_CONF_RSP_PENDING 0x0004 |
| #define L2C_SIG_CONF_RSP_FLOW_SPEC_REJ 0x0005 |
| |
| #define L2C_SIG_CONF_FLAG_C 0x0001 |
| |
| #define L2C_SIG_DISC_REQ 0x06 |
| struct l2cSigDiscReq { |
| uint16_t dcid; |
| uint16_t scid; |
| } __packed; |
| |
| #define L2C_SIG_DISC_RSP 0x07 |
| struct l2cSigDiscRsp { |
| uint16_t dcid; |
| uint16_t scid; |
| } __packed; |
| |
| #define L2C_SIG_ECHO_REQ 0x08 |
| /* all data is optional */ |
| |
| #define L2C_SIG_ECHO_RSP 0x09 |
| /* all data is optional */ |
| |
| #define L2C_SIG_INFO_REQ 0x0A |
| struct l2cSigInfoReq { |
| uint16_t infoType; |
| } __packed; |
| |
| #define L2C_SIG_INFO_TYPE_UNCONN_MTU 0x0001 |
| #define L2C_SIG_INFO_TYPE_EXT_FTRS 0x0002 |
| #define L2C_SIG_INFO_TYPE_FIXED_CHS 0x0003 |
| |
| #define L2C_SIG_INFO_RSP 0x0B |
| struct l2cSigInfoRsp { |
| uint16_t infoType; |
| uint16_t result; |
| /* optional data */ |
| } __packed; |
| |
| #define L2C_SIG_INFO_RESULT_OK 0x0000 |
| #define L2C_SIG_INFO_RESULT_NO 0x0001 |
| |
| |
| #define L2C_SIG_CH_CREAT_REQ 0x0C |
| struct l2cSigChCreatReq { |
| psm_t psm; |
| uint16_t scid; |
| uint8_t controller; |
| } __packed; |
| |
| #define L2C_SIG_CH_CREAT_RSP 0x0D |
| struct l2cSigChCreatRsp { |
| uint16_t dcid; |
| uint16_t scid; |
| uint16_t result; |
| uint16_t status; |
| } __packed; |
| |
| #define L2C_SIG_CH_MOVE_REQ 0x0E |
| struct l2cSigChMoveReq { |
| uint16_t icid; |
| uint8_t controller; |
| } __packed; |
| |
| #define L2C_SIG_CH_MOVE_RSP 0x0F |
| struct l2cSigChMoveRsp { |
| uint16_t icid; |
| uint16_t result; |
| } __packed; |
| #define L2C_SIG_CH_MOVE_CNF_REQ 0x10 |
| struct l2cSigChMoveCnfReq { |
| uint16_t icid; |
| uint16_t result; |
| } __packed; |
| |
| #define L2C_SIG_CH_MOVE_CNF_RSP 0x11 |
| struct l2cSigChMoveCnfRsp { |
| uint16_t icid; |
| } __packed; |
| |
| #define L2C_SIG_CONN_UPDT_REQ 0x12 |
| struct l2cSigConnUpdtReq { |
| uint16_t intMin; |
| uint16_t intMax; |
| uint16_t latency; |
| uint16_t timeoutMult; |
| } __packed; |
| |
| |
| #define L2C_SIG_CONN_UPDT_RSP 0x13 |
| struct l2cSigConnUpdtRsp { |
| uint16_t result; |
| } __packed; |
| |
| #define L2C_SIG_CONN_UPDT_OK 0x0000 |
| #define L2C_SIG_CONN_UPDT_NO 0x0001 |
| |
| |
| |
| /* low level L2CAP frame */ |
| struct l2cFrmHdr { /* header of all frames */ |
| uint16_t len; |
| uint16_t cid; |
| } __packed; |
| |
| /* L2CAP signalling channel frame */ |
| struct l2cSigFrm { |
| uint8_t code; |
| uint8_t ident; |
| uint16_t len; |
| } __packed; |
| |
| /* our per-ACL-connection structure */ |
| struct l2cAclConn { |
| struct l2cAclConn *next; |
| struct l2cAclConn *prev; |
| uniq_t handle; |
| uniq_t openTimer; |
| uniq_t teardownTimer; |
| hci_conn_t conn; |
| sg reassSg; |
| |
| uint32_t peerFeatures; |
| uint64_t peerFixedChs; |
| uint8_t ident; /* next ident to use */ |
| uint16_t nextChan; /* a guess to a currently-free channel */ |
| uint16_t connectionlessMtu; |
| uint16_t sigMtu; |
| struct bt_addr peerAddr; /* who the peer is */ |
| struct bt_addr selfAddr; /* who we were acting as when we made the connection */ |
| uint8_t state; |
| uint8_t isMaster : 1; |
| uint8_t encrypted : 1; |
| uint8_t mitmSafe : 1; |
| |
| uint8_t reqIdent; /* ident for an outstanding request */ |
| |
| l2c_handle_t securityManager; /* 0 if none yet */ |
| |
| struct { |
| union { /* LE-only data */ |
| l2cConnUpdateDoneCbk connUpdtDoneCbk; |
| void *connUpdtDoneCbkData; |
| uint8_t connUpdtRequestIdent; /* 0 if none inflight */ |
| }; |
| /* EDR-only data here, if any */ |
| }; |
| }; |
| |
| struct l2cSigPduSendItem { |
| struct l2cSigPduSendItem *next; |
| struct l2cSigPduSendItem *prev; |
| hci_conn_t aclConnID; |
| sg pdu; |
| uint16_t cid; |
| bool hasL2cHdr; /* if we've added a header already, we can no longer merge */ |
| }; |
| |
| /* our work struct for service worker theread */ |
| union l2cPsmOrChan { |
| psm_t psm; |
| uint16_t chan; |
| }; |
| |
| struct l2cSvcWork { |
| int type; |
| union { |
| struct { |
| l2c_handle_t handle; |
| union l2cPsmOrChan which; |
| } i_alloc; |
| struct { |
| l2c_handle_t handle; |
| sg payload; |
| uint32_t size; |
| psm_t psm; |
| } cl_rx; |
| struct { |
| union l2cPsmOrChan which; |
| void (*doneCbk)(void*); |
| void *cbkData; |
| } unreg; |
| struct { |
| l2cStateCbk stateCbk; |
| void *userData; |
| void *instance; |
| uint8_t state; |
| uint32_t len; |
| } state; |
| struct { |
| l2cConnUpdateDoneCbk doneCbk; |
| void *cbkData; |
| bool success; |
| } connUpdtCbk; |
| struct { |
| struct bt_addr addr; |
| //todo: more params for non-default conns? |
| } linkOpen; |
| struct { |
| hci_conn_t aclConn; |
| } linkClose; |
| struct { |
| hci_conn_t aclConnID; |
| uint16_t minInt; |
| uint16_t maxInt; |
| uint16_t lat; |
| uint16_t to; |
| } connUpdt; |
| struct { |
| hci_conn_t aclConnID; |
| bool demandMitmSafe; |
| } demandEncr; |
| struct { |
| bool useLeBuffers; |
| } tryTx; |
| struct { |
| L2capDefaultSecurityManagerCbk cbk; |
| hci_conn_t hciConn; |
| uint64_t randomNum; |
| uint16_t diversifier; |
| } defaultSm; |
| struct { |
| sem_t *sem; |
| } flush; |
| }; |
| }; |
| #define L2C_WORK_PSM_INST_ALLOC 0 |
| #define L2C_WORK_PSM_CONNECTIONLESS_RX 1 |
| #define L2C_WORK_PSM_UNREGISTER 2 /* does not call service itself */ |
| #define L2C_WORK_FIXEDCH_INST_ALLOC 3 |
| #define L2C_WORK_FIXEDCH_UNREGISTER 4 /* does not call service itself */ |
| #define L2C_WORK_STATE_CBK 5 |
| #define L2C_WORK_CONN_UPDT_CBK 6 |
| #define L2C_WORK_ACL_LINK_OPEN 7 |
| #define L2C_WORK_ACL_LINK_CLOSE 8 |
| #define L2C_WORK_ACL_CONN_UPDT 9 |
| #define L2C_WORK_ACL_DEMAND_ENCR 10 |
| #define L2C_WORK_TRY_HCI_TX 11 |
| #define L2C_WORK_DEFAULT_SM_CALL 12 |
| #define L2C_WORK_FLUSH 13 |
| |
| /* our per-service struct for PSM-based services */ |
| struct l2cPsmSvc { |
| struct l2cPsmSvc *next; |
| struct l2cPsmSvc *prev; |
| psm_t psm; |
| struct l2cServicePsmDescriptor desc; |
| uint32_t beingRemoved : 1; |
| }; |
| |
| /* our per-service struct for PSM-based services */ |
| struct l2cFixedChSvc { |
| struct l2cFixedChSvc *next; |
| struct l2cFixedChSvc *prev; |
| uint16_t chan; |
| struct l2cServiceFixedChDescriptor desc; |
| uint32_t beingRemoved : 1; |
| }; |
| |
| struct l2cAdvFrag { |
| sg data; |
| uint8_t sar; |
| }; |
| |
| /* our per-l2c connection struct */ |
| /* also used for fixed ch "connections" */ |
| struct l2cConn { |
| uniq_t handle; |
| struct l2cConn *next; |
| struct l2cConn *prev; |
| uniq_t timer; |
| |
| l2cStateCbk stateCbk; |
| void *userData; |
| void *instance; |
| |
| uniq_t acl; /* the acl connection */ |
| uint8_t ident; /* for control channel ops */ |
| |
| uint8_t state; |
| uint8_t mode; /* L2C_MODE_* */ |
| |
| psm_t psm; /* 0 for fixedCh */ |
| uint16_t localCh; /* both this & remCh ==0 means */ |
| uint16_t remCh; /* this is connectionless conn struct */ |
| uint16_t ourMtu; /* for our side */ |
| uint16_t theirMtu; /* for other side */ |
| |
| uint8_t weOpened : 1; /* 1 if we asked for this, 0 if peer */ |
| uint8_t inUse : 1; /* 1 if connection is in use, 0 else */ |
| uint8_t fcs : 1; /* 1 if FCS is on */ |
| |
| sg queuedRx; |
| |
| union { /* per-state info */ |
| struct { /* config */ |
| uint8_t cfgTxState; |
| uint8_t cfgRxState; |
| }cfg; |
| }; |
| |
| /* advanced mode structures for TX */ |
| struct l2cAdvFrag advTxFrags[L2C_RETR_NUM_BUFFERS]; /* our tx buffers */ |
| sg advEnqueuedPacket; /* may be more than a fragment long */ |
| uint8_t advTxSendPos; /* where next send will come from */ |
| uint8_t advTxWritePos; /* where the next write will go */ |
| uint8_t advTxFirstUnacked; /* what the first non-acked packet was */ |
| uint8_t advTxWindowSz; /* how many outstanding not-acked packets there can be in our TX path */ |
| bool advTxShutUp; /* set if peer wants us to stop sending *DATA* */ |
| bool advTxWeOweAck; /* do we owe them an ack */ |
| uint16_t advTxMps; /* max payload per fragment */ |
| |
| /* advanced mode structures for RX */ |
| struct l2cAdvFrag advRxFrags[L2C_RETR_NUM_BUFFERS]; /* our rx buffers */ |
| sg advTxReassBuf; /* our current work on reassembling a frame */ |
| uint16_t advTxReassExpectLen; /* expected extra length of packet we're reassembling */ |
| uint8_t advRxFirstMissing; /* first fragment we need */ |
| uint8_t advRxWindowSz; /* how many outstanding not-acked packets there can be in our RX path */ |
| bool advRxWeOweAck; /* do we owe them an ack */ |
| }; |
| |
| #define L2C_CONN_STATE_CONN_WAIT 0 /* waiting for ACL link up */ |
| #define L2C_CONN_STATE_RX_OPEN_SVC 1 /* call to service in pipeline */ |
| #define L2C_CONN_STATE_TX_OPEN_TXED 2 /* request sent to other side */ |
| #define L2C_CONN_STATE_ENCR_WAIT 3 /* waiting for encr/auth */ |
| #define L2C_CONN_STATE_ATHORIZ_WAIT 4 /* waiting for user athorization */ |
| #define L2C_CONN_STATE_CFG 5 /* configuration ongoing */ |
| #define L2C_CONN_STATE_ESTABLISHED 6 /* connected */ |
| #define L2C_CONN_STATE_DIE_REQD 7 /* we closed, call to svc in pipeline */ |
| #define L2C_CONN_STATE_DIE_RXED 8 /* other side closed, call to svc in pipeline */ |
| #define L2C_CONN_STATE_TEARDOWN 9 /* tearing down */ |
| |
| #define L2C_ACL_STATE_PENDING 0 /* not yet established */ |
| #define L2C_ACL_STATE_CFG 1 /* we are still finding out peer config. do not yet start outbound connections */ |
| #define L2C_ACL_STATE_ESTABLISHED 2 /* we can now start outbound connections */ |
| #define L2C_ACL_STATE_TEARDOWN 3 /* going down - do not use */ |
| |
| #define L2C_CFG_STATE_INITIAL 0 |
| #define L2C_CFG_STATE_REQ_SENT 1 |
| #define L2C_CFG_STATE_CFG_ACCEPTED 2 |
| |
| |
| /* work for seg send thread */ |
| struct l2cSigFrameWork { |
| hci_conn_t aclConnID; |
| void *data; |
| uint32_t len; |
| bool isLE; |
| }; |
| |
| /* some fwd declarations */ |
| static uint8_t l2cSigSendErrCmdNotUnderstood(struct l2cAclConn *aclConn, uint8_t ident); |
| static uint8_t l2cSigSendErrInvalidCid(struct l2cAclConn *aclConn, uint8_t ident, uint16_t rxedDcid, uint16_t rxedScid); |
| static uint16_t l2cAclFreeCid(struct l2cAclConn *aclConn); |
| static uint8_t l2cSigSendConnectionRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t remCh, uint16_t result, uint16_t status); |
| static struct l2cConn* l2cConnFindByHandle(l2c_handle_t handle); |
| static void l2cConnStructDelete(struct l2cConn *conn); |
| static void l2cAclConnStructDelete(struct l2cAclConn *conn); |
| static void l2cPsmSendConfig(struct l2cAclConn *aclConn, struct l2cConn *conn); |
| static uint8_t l2cSigSendConfigReq(struct l2cAclConn *aclConn, uint16_t remCh, uint16_t flags, const void *cfgData, uint16_t cfgLen); |
| static uint8_t l2cSigSendConfigRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t remCh, uint16_t flags, uint16_t result, const void *cfgData, uint16_t cfgLen); |
| static uint8_t l2cSigSendInfoRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t infoType, uint16_t result, const void *infoData, uint16_t infoLen); |
| static uint8_t l2cSigSendEchoRsp(struct l2cAclConn *aclConn, uint8_t ident, const void *echoData, uint16_t echoLen); |
| static uint8_t l2cSigSendConnUpdateRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t result); |
| static uint8_t l2cSigSendConnUpdateReq(struct l2cAclConn *aclConn, uint16_t intMin, uint16_t intMax, uint16_t latency, uint16_t timeoutMult); |
| static uint8_t l2cSigSendDiscRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t remCh); |
| static void l2cConnRequestClose(struct l2cAclConn* aclConn, struct l2cConn *conn, bool tellOtherSide); |
| static void l2cAclDownTimerStart(struct l2cAclConn *aclConn); |
| static void l2cAclDownTimerStop(struct l2cAclConn *aclConn); |
| static void l2cAclLinkDownInt(struct l2cAclConn *aclConn, bool tellOtherSide); |
| static void l2cPsmCfgStart(struct l2cAclConn *aclConn, struct l2cConn *conn); |
| static void l2cTryMakeSendProgress(bool forLeBuffers); |
| static uint8_t l2cSigSendInfoReq(struct l2cAclConn *aclConn, uint16_t infoType); |
| static uint16_t l2cUtilFcs(uint16_t fcs, const void *data, uint32_t len); |
| static uint16_t l2cUtilFcsSg(uint16_t fcs, sg s); |
| static bool l2cAdvMayTx(void); |
| static bool l2cAdvMayTxLocked(void); |
| static uint8_t l2cAdvUserTx(struct l2cConn *conn, sg data); /* -> L2C_TX_* */ |
| static uint8_t l2cDataSendOnAclLink(struct l2cAclConn *aclConn, const void *hdr, uint32_t hdrLen, sg data); |
| |
| |
| /* our state */ |
| static bool mChipJointBuffers; |
| static uint16_t mChipBufLenEdr; |
| static uint16_t mChipBufLenLe; |
| static uint16_t mChipBufNumLe; |
| |
| static pthread_t mSvcWorker; |
| static struct workQueue *mServiceWork = NULL; |
| |
| static pthread_mutex_t mSvcsLock = PTHREAD_MUTEX_INITIALIZER; |
| static struct l2cPsmSvc *mPsmSvcs = NULL; |
| static struct l2cFixedChSvc *mFixedSvcs = NULL; |
| static psm_t mNextPsm; |
| |
| static pthread_mutex_t mConnsLock = PTHREAD_MUTEX_INITIALIZER; |
| static struct l2cAclConn *mAclConns = NULL; |
| static struct l2cConn *mConns = NULL; |
| static struct l2cConn *mNextToTx = NULL; |
| static sg mQueueBufLe = NULL; |
| static sg mQueueBufEdr = NULL; |
| static bool mQueueFirstLe; |
| static bool mQueueFirstEdr; |
| static hci_conn_t mQueueConnLe = 0; |
| static hci_conn_t mQueueConnEdr = 0; |
| static struct l2cSigPduSendItem *sigSendHeadEdr = NULL; |
| static struct l2cSigPduSendItem *sigSendTailEdr = NULL; |
| static struct l2cSigPduSendItem *sigSendHeadLe = NULL; |
| static struct l2cSigPduSendItem *sigSendTailLe = NULL; |
| |
| static struct multiNotifList *mWriteNotif = NULL; |
| |
| static pthread_mutex_t mSecMgrLock = PTHREAD_MUTEX_INITIALIZER; |
| static L2capDefaultSecurityManagerCbk mDefaultSecMgr = NULL; |
| |
| |
| |
| /* |
| * FUNCTION: l2cAclConnFindById |
| * USE: Find a l2cConn structure for a given ACL connection ID |
| * PARAMS: aclConn - the ACL connection ID |
| * RETURN: the found struct or null |
| * NOTES: requires mConnsLock held |
| */ |
| static struct l2cAclConn* l2cAclConnFindById(hci_conn_t aclConn) |
| { |
| struct l2cAclConn *c = mAclConns; |
| |
| while (c && c->conn != aclConn) |
| c = c->next; |
| |
| return c; |
| } |
| |
| /* |
| * FUNCTION: l2cAclConnFindByHandle |
| * USE: Find a l2cConn structure for a given a UniqID |
| * PARAMS: uniq - the UniqID |
| * RETURN: the found struct or null |
| * NOTES: requires mConnsLock held |
| */ |
| static struct l2cAclConn* l2cAclConnFindByHandle(uniq_t handle) |
| { |
| struct l2cAclConn *c = mAclConns; |
| |
| while (c && c->handle != handle) |
| c = c->next; |
| |
| return c; |
| } |
| |
| /* |
| * FUNCTION: l2cAclConnFindByAddr |
| * USE: Find a l2cConn structure for a given a peer addr |
| * PARAMS: addr - the addr |
| * RETURN: the found struct or null |
| * NOTES: requires mConnsLock held |
| */ |
| static struct l2cAclConn* l2cAclConnFindByAddr(const struct bt_addr* addr) |
| { |
| struct l2cAclConn *c = mAclConns; |
| |
| while (c && memcmp(&c->peerAddr, addr, sizeof(struct bt_addr))) |
| c = c->next; |
| |
| return c; |
| } |
| |
| /* |
| * FUNCTION: l2cPsmValid |
| * USE: Check a PSM for validity |
| * PARAMS: psm - the PSM |
| * RETURN: true if valid, else false |
| * NOTES: we only support PSMs of 16-bit size |
| */ |
| static bool l2cPsmValid(psm_t psm) |
| { |
| /* PSMs are odd */ |
| if (!(psm & 1)) |
| return false; |
| |
| /* LSB of top byte shall be zero */ |
| if (psm & 0x0100) |
| return false; |
| |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkAlloc |
| * USE: Allocate a work item for the service worker |
| * PARAMS: type - the type of work |
| * extraLen - how many bytes extra to allocate (for data past work struct) |
| * RETURN: work item or null |
| * NOTES: |
| */ |
| static struct l2cSvcWork* l2cSvcWorkAlloc(int type, uint32_t extraLen) |
| { |
| struct l2cSvcWork* w = (struct l2cSvcWork*)calloc(1, sizeof(struct l2cSvcWork) + extraLen); |
| |
| if (w) |
| w->type = type; |
| else |
| loge("Failed to allocate service work item\n"); |
| |
| return w; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkSchedulePsmAlloc |
| * USE: Schedule a service allocation |
| * PARAMS: psm - the psm which this is for |
| * handle - the handle to pass to the service alloc handler |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread |
| */ |
| static bool l2cSvcWorkSchedulePsmAlloc(psm_t psm, l2c_handle_t handle) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_PSM_INST_ALLOC, 0); |
| |
| if (!w) |
| return false; |
| |
| w->i_alloc.handle = handle; |
| w->i_alloc.which.psm = psm; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for service instance allocation!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkSchedulePsmConnectionlessRx |
| * USE: Schedule a service data RX for connectionless data |
| * PARAMS: psm - the psm which this is for |
| * handle - the handle to pass to the service RX connectionless handler |
| * payload - the data that arrived |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. Buffer is copied |
| */ |
| static bool l2cSvcWorkSchedulePsmConnectionlessRx(psm_t psm, l2c_handle_t handle, sg payload) |
| { |
| struct l2cSvcWork* w; |
| |
| w = l2cSvcWorkAlloc(L2C_WORK_PSM_CONNECTIONLESS_RX, 0); |
| if (!w) { |
| return false; |
| } |
| |
| w->cl_rx.handle = handle; |
| w->cl_rx.payload = payload; |
| w->cl_rx.psm = psm; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for service connectionless RX!\n"); |
| sgFree(payload); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleFixedChAlloc |
| * USE: Schedule a service allocation |
| * PARAMS: chan - the channel this is for |
| * handle - the handle to pass to the service alloc handler |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread |
| */ |
| static bool l2cSvcWorkScheduleFixedChAlloc(uint16_t chan, l2c_handle_t handle) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_FIXEDCH_INST_ALLOC, 0); |
| |
| if (!w) |
| return false; |
| |
| w->i_alloc.handle = handle; |
| w->i_alloc.which.chan = chan; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for service instance allocation!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleFixedChUnregister |
| * USE: Schedule a service unregistering |
| * PARAMS: chan - the channel this is for |
| * doneCbk - callback to call when done (or NULL) |
| * cbkData - data to pass to callback |
| * RETURN: false on immediate error |
| * NOTES: actual work will happen in the worker thread |
| */ |
| static bool l2cSvcWorkScheduleFixedChUnregister(uint16_t chan, void (*doneCbk)(void*), void *cbkData) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_FIXEDCH_UNREGISTER, 0); |
| |
| if (!w) |
| return false; |
| |
| w->unreg.doneCbk = doneCbk; |
| w->unreg.cbkData = cbkData; |
| w->unreg.which.chan = chan; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for service unregistering!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkSchedulePsmUnregister |
| * USE: Schedule a service unregistering |
| * PARAMS: psm - the psm which this is for |
| * doneCbk - callback to call when done (or NULL) |
| * cbkData - data to pass to callback |
| * RETURN: false on immediate error |
| * NOTES: actual work will happen in the worker thread |
| */ |
| static bool l2cSvcWorkSchedulePsmUnregister(psm_t psm, void (*doneCbk)(void*), void *cbkData) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_PSM_UNREGISTER, 0); |
| |
| if (!w) |
| return false; |
| |
| w->unreg.doneCbk = doneCbk; |
| w->unreg.cbkData = cbkData; |
| w->unreg.which.psm = psm; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for service unregistering!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleStateCallDirect |
| * USE: Schedule a "state" callback with directly-passed params |
| * PARAMS: stateCbk - the callback to call |
| * userData - data pointer to pass to the callback |
| * instance - second data pointer to pass to the callback |
| * state - state vaklue to pass to the callback |
| * data - buffer a copy of which is passed to the callback |
| * len - the length of the buffer |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. cannot supply a buffer! |
| */ |
| static bool l2cSvcWorkScheduleStateCallDirect(l2cStateCbk stateCbk, void *userData, void *instance, uint8_t state, const void *data, uint32_t len) |
| { |
| struct l2cSvcWork* w; |
| |
| w = l2cSvcWorkAlloc(L2C_WORK_STATE_CBK, len); |
| if (!w) |
| return false; |
| |
| w->state.stateCbk = stateCbk; |
| w->state.userData = userData; |
| w->state.instance = instance; |
| w->state.state = state; |
| w->state.len = len; |
| |
| memcpy(w + 1, data, len); |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for state call!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleStateCall |
| * USE: Schedule a "state" callback for a connection |
| * PARAMS: conn - the connection |
| * state - state vaklue to pass to the callback |
| * data - buffer a copy of which is passed to the callback |
| * len - the length of the buffer |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. Buffer is copied |
| */ |
| static bool l2cSvcWorkScheduleStateCall(struct l2cConn* conn, uint8_t state, const void *data, uint32_t len) |
| { |
| return l2cSvcWorkScheduleStateCallDirect(conn->stateCbk, conn->userData, conn->instance, state, data, len); |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleConnUpdtCbk |
| * USE: Schedule a "connection update" callback |
| * PARAMS: doneCbk - the callback to call |
| * cbkData - data pointer to pass to the callback |
| * success - success sttaus to pass to the callback |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleConnUpdtCbk(l2cConnUpdateDoneCbk doneCbk, void *cbkData, bool success) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_CONN_UPDT_CBK, 0); |
| if (!w) |
| return false; |
| |
| w->connUpdtCbk.doneCbk = doneCbk; |
| w->connUpdtCbk.cbkData = cbkData; |
| w->connUpdtCbk.success = success; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for connection update callback!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleAclLinkOpen |
| * USE: Schedule an ACL link open call to the lower layer |
| * PARAMS: addr - the address of peer to connect to |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleAclLinkOpen(const struct bt_addr *addr) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_ACL_LINK_OPEN, 0); |
| if (!w) |
| return false; |
| |
| w->linkOpen.addr = *addr; |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for ACL link open!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleAclLinkClose |
| * USE: Schedule an ACL link close call to the lower layer |
| * PARAMS: aclConn - the connection to close |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleAclLinkClose(hci_conn_t aclConn) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_ACL_LINK_CLOSE, 0); |
| if (!w) |
| return false; |
| |
| w->linkClose.aclConn = aclConn; |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for ACL link close!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleAclConnUpdt |
| * USE: Schedule a connection update call to the lower layer |
| * PARAMS: aclConnID - the connection ID |
| * minInt - update command parameter |
| * maxInt - update command parameter |
| * lat - update command parameter |
| * to - update command parameter |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleAclConnUpdt(hci_conn_t aclConnID, uint16_t minInt, uint16_t maxInt, uint16_t lat, uint16_t to) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_ACL_CONN_UPDT, 0); |
| if (!w) |
| return false; |
| |
| w->connUpdt.aclConnID = aclConnID; |
| w->connUpdt.minInt = minInt; |
| w->connUpdt.maxInt = maxInt; |
| w->connUpdt.lat = lat; |
| w->connUpdt.to = to; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for ACL connection update!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleAclDemandEncr |
| * USE: Schedule a connection encryption request |
| * PARAMS: aclConnID - the connection ID |
| * demandMitmSafe - demand authentication too? |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleAclDemandEncr(hci_conn_t aclConnID, bool demandMitmSafe) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_ACL_DEMAND_ENCR, 0); |
| if (!w) |
| return false; |
| |
| w->demandEncr.aclConnID = aclConnID; |
| w->demandEncr.demandMitmSafe = demandMitmSafe; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for ACL connection encprypt!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleTryTx |
| * USE: Schedule an attempt to do some TX |
| * PARAMS: useLeBuffers - which buffer set to try TX on |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleTryTx(bool useLeBuffers) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_TRY_HCI_TX, 0); |
| if (!w) |
| return false; |
| |
| w->tryTx.useLeBuffers = useLeBuffers; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for ACL TX!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleDefaultSmCall |
| * USE: Schedule an attempt to call the dafult security manager |
| * PARAMS: mgrCbk - the manager to call |
| * hciConn - the hci acl connection |
| * randomNum - the random number gotten from peer |
| * diversifier - the diversifier gotten from peer |
| * RETURN: false on immediate error |
| * NOTES: actual call will happen in the worker thread. |
| */ |
| static bool l2cSvcWorkScheduleDefaultSmCall(L2capDefaultSecurityManagerCbk mgrCbk, hci_conn_t hciConn, uint64_t randomNum, uint16_t diversifier) |
| { |
| struct l2cSvcWork* w = l2cSvcWorkAlloc(L2C_WORK_DEFAULT_SM_CALL, 0); |
| if (!w) |
| return false; |
| |
| w->defaultSm.cbk = mgrCbk; |
| w->defaultSm.hciConn = hciConn; |
| w->defaultSm.randomNum = randomNum; |
| w->defaultSm.diversifier = diversifier; |
| |
| if (workQueuePut(mServiceWork, w)) |
| return true; |
| |
| loge("Failed to enqueue service work for ACL default SM call!\n"); |
| free(w); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorkScheduleFlush |
| * USE: Flush the workQueue |
| * PARAMS: NONE |
| * RETURN: success |
| * NOTES: blocks till all existing work items have been finished |
| */ |
| static bool l2cSvcWorkScheduleFlush(void) |
| { |
| struct l2cSvcWork *w = l2cSvcWorkAlloc(L2C_WORK_FLUSH,0); |
| bool ret = false; |
| sem_t sem; |
| |
| if (!w) |
| return false; |
| |
| if (sem_init(&sem, 0, 0)) { |
| free(w); |
| loge("Failed to alloc workQ flush sem\n"); |
| return false; |
| } |
| |
| w->flush.sem = &sem; |
| |
| if (workQueuePut(mServiceWork, w)) { |
| r_sem_wait(&sem); |
| ret = true; |
| } else |
| free(w); |
| |
| sem_destroy(&sem); |
| |
| return ret; |
| } |
| |
| |
| /* |
| * FUNCTION: l2cConnTimeout |
| * USE: Timer timeout callback for per-connection timers |
| * PARAMS: timerH - the timer handle |
| * connH - the connection handle |
| * RETURN: NONE |
| * NOTES: |
| */ |
| static void l2cConnTimeout(uniq_t timerH, uint64_t connH) |
| { |
| struct l2cConn *conn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cConnFindByHandle(connH); |
| if (!conn) |
| logw("Timer callback "HANDLEFMT" for nonexistent connection "HANDLEFMT"\n", HANDLECNV(timerH), HANDLECNV(connH)); |
| else if (conn->timer != timerH) |
| logd("Stale timer callback "HANDLEFMT" for connection "HANDLEFMT"\n", HANDLECNV(timerH), HANDLECNV(connH)); |
| else { |
| /* We found a genuine timeout - figure out what kind by looking at state |
| * Note that this is purely for logging purposes, since we handle them all |
| * the same - by closing the connection. |
| */ |
| switch (conn->state) { |
| case L2C_CONN_STATE_RX_OPEN_SVC: |
| case L2C_CONN_STATE_CONN_WAIT: |
| case L2C_CONN_STATE_ESTABLISHED: |
| case L2C_CONN_STATE_DIE_REQD: |
| case L2C_CONN_STATE_DIE_RXED: |
| case L2C_CONN_STATE_TEARDOWN: |
| loge("Unexpected timeout in state %d for "HANDLEFMT" - closing connection\n", conn->state, HANDLECNV(connH)); |
| break; |
| case L2C_CONN_STATE_TX_OPEN_TXED: |
| logi("Connection open timeout for "HANDLEFMT" - closing connection\n", HANDLECNV(connH)); |
| break; |
| case L2C_CONN_STATE_CFG: |
| logi("Connection configuration timeout for "HANDLEFMT" - closing connection\n", HANDLECNV(connH)); |
| break; |
| case L2C_CONN_STATE_ENCR_WAIT: |
| logi("Connection open after auth timeout for "HANDLEFMT" - closing connection\n", HANDLECNV(connH)); |
| break; |
| default: |
| loge("Unknown state %d for "HANDLEFMT" at timeout time - closing connection\n", conn->state, HANDLECNV(connH)); |
| break; |
| } |
| l2cConnRequestClose(NULL, conn, true); |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cConnTimer |
| * USE: Set or clear a per-L2C-connection timer |
| * PARAMS: conn - the connection |
| * timeout - time to set ot 0 to clear |
| * RETURN: NONE |
| * NOTES: call mConnsLock held |
| */ |
| static void l2cConnTimer(struct l2cConn *conn, uint64_t timeout) |
| { |
| if (conn->timer) { |
| logd("Removing existing timer "HANDLEFMT" for conn "HANDLEFMT"\n", HANDLECNV(conn->timer), HANDLECNV(conn->handle)); |
| timerCancel(conn->timer); |
| conn->timer = 0; |
| } |
| if (timeout) { |
| logd("Setting a timer for %llu ms for conn "HANDLEFMT"\n", (unsigned long long)timeout, HANDLECNV(conn->handle)); |
| conn->timer = timerSet(timeout, l2cConnTimeout, conn->handle); |
| if (!conn->timer) |
| loge("Failed to set per-conn timer\n"); |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cNewConnStruct |
| * USE: Create a new l2cConn and prepopulate with some values |
| * PARAMS: acl - acl connection handle |
| * state - state value to set |
| * localCh - localCh value to set |
| * remCh - remCh value to set |
| * psm - psm value to set |
| * ourMtu - the MTU we're willing to accept |
| * remCh - the remote channel used to make the request |
| * weOpened - true if we're opening this conn, false if peer |
| * RETURN: The struct or NULL on error |
| * NOTES: |
| */ |
| static struct l2cConn* l2cNewConnStruct(uint8_t state, uniq_t acl, uint16_t localCh, uint16_t remCh, psm_t psm, uint16_t ourMtu, bool weOpened) |
| { |
| struct l2cConn* conn = (struct l2cConn*)calloc(1, sizeof(struct l2cConn)); |
| |
| if (!conn) |
| return NULL; |
| |
| conn->acl = acl; |
| conn->psm = psm; |
| conn->theirMtu = 0xFFFF; |
| conn->ourMtu = ourMtu; |
| conn->handle = uniqGetNext(); |
| conn->remCh = remCh; |
| conn->localCh = localCh; |
| conn->state = state; |
| conn->weOpened = weOpened ? 1 : 0; |
| conn->timer = 0; |
| |
| return conn; |
| } |
| |
| /* |
| * FUNCTION: l2cHandleConnReq |
| * USE: Process request to open a PSM channel |
| * PARAMS: aclConn - the per-ACL-connection structure |
| * ident - the ident to use for response |
| * psm - the requested psm |
| * remCh - the remote channel used to make the request |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleConnReq(struct l2cAclConn *aclConn, uint8_t ident, psm_t psm, uint16_t remCh) |
| { |
| struct l2cPsmSvc *svc = NULL; |
| struct l2cConn *conn = NULL; |
| uint16_t localCh = 0; |
| bool enqueued = false; |
| uint16_t result; |
| |
| if (!l2cPsmValid(psm)) |
| goto out_nolocks; |
| |
| pthread_mutex_lock(&mSvcsLock); |
| svc = mPsmSvcs; |
| while (svc && (svc->psm != psm || svc->beingRemoved || !svc->desc.serviceInstanceAlloc)) |
| svc = svc->next; |
| if (!svc) |
| goto out_svcs_lock; |
| |
| conn = l2cNewConnStruct(L2C_CONN_STATE_RX_OPEN_SVC, aclConn->handle, 0, remCh, psm, svc->desc.mtu, false); |
| if (!conn) { |
| loge("Failed to allocate an L2C conn\n"); |
| goto out_svcs_lock; |
| } |
| localCh = l2cAclFreeCid(aclConn); |
| if (!localCh) { |
| logw("No free channels for acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| free(conn); |
| goto out_svcs_lock; |
| } |
| conn->localCh = localCh; |
| conn->ident = ident; |
| conn->inUse = 1; |
| l2cAclDownTimerStop(aclConn); |
| |
| /* safe to call this enqueue here, since worker thread cannot work on it till we release this lock */ |
| enqueued = l2cSvcWorkSchedulePsmAlloc(psm, conn->handle); |
| if (enqueued) { |
| conn->next = mConns; |
| if (mConns) |
| mConns->prev = conn; |
| mConns = conn; |
| } else { |
| free(conn); |
| } |
| |
| out_svcs_lock: |
| pthread_mutex_unlock(&mSvcsLock); |
| |
| out_nolocks: |
| if (!svc) { |
| result = L2C_SIG_CONN_RSP_BAD_PSM; |
| logi("Request for connect to unknown psm %d\n", psm); |
| } else if (!conn) { |
| result = L2C_SIG_CONN_RSP_NO_RSRC; |
| logi("Couldnt allocate a connection struct for incoming psm %d open request\n", psm); |
| } else if (!localCh) { |
| result = L2C_SIG_CONN_RSP_NO_RSRC; |
| logi("Couldnt find a local channel for incoming psm %d open request\n", psm); |
| } else if (!enqueued) { |
| result = L2C_SIG_CONN_RSP_NO_RSRC; |
| logi("Couldnt enqueue a service alloc request for incoming psm %d open request\n", psm); |
| } else { |
| logd("Scheduled service open for psm %d\n", psm); |
| return; |
| } |
| |
| if (!l2cSigSendConnectionRsp(aclConn, ident, 0, remCh, result, 0)) |
| loge("Failed to sent negative connection request response frame\n"); |
| } |
| |
| /* |
| * FUNCTION: l2cHandleConnRsp |
| * USE: Randle a response to a open PSM channel request we had sent |
| * PARAMS: aclConn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * remCh - remote channel number |
| * localCh -local channel number |
| * result - the result the remote side sent |
| * status - the status from the remote side |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held. May destroy your conn |
| */ |
| static void l2cHandleConnRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t remCh, uint16_t localCh, uint16_t result, uint16_t status) |
| { |
| struct l2cConn *conn = NULL; |
| |
| |
| conn = mConns; |
| while (conn && (conn->acl != aclConn->handle || conn->localCh != localCh)) |
| conn = conn->next; |
| |
| if (!conn) { |
| logw("Unexpected Connection Response id %d for ch %d %d with result %d\n", ident, localCh, remCh, result); |
| return; |
| } |
| |
| if (ident != conn->ident) { |
| logi("RXed a Connection Response witrh an invalid ident %d (expected %d). Dropping\n", ident, conn->ident); |
| return; |
| } |
| |
| if (!conn->psm) { |
| logw("Unexpected Connection Response for a FixedCh conn chan %d\n", localCh); |
| return; |
| } |
| |
| if (conn->state != L2C_CONN_STATE_TX_OPEN_TXED) { |
| logw("Connection Response for connection in bad state %d %d for ch %d %d with result %d\n", conn->state, ident, localCh, remCh, result); |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to schedule 'closed' state call\n"); |
| l2cConnStructDelete(conn); |
| return; |
| } |
| |
| l2cConnTimer(conn, 0); |
| |
| if (result == L2C_SIG_CONN_RSP_BAD_PSM || result == L2C_SIG_CONN_RSP_BAD_SEC || result == L2C_SIG_CONN_RSP_NO_RSRC) { |
| logi("Other side refused our connection for psm %d\n", conn->psm); |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to schedule 'closed' state call\n"); |
| l2cConnStructDelete(conn); |
| return; |
| } |
| |
| if (result == L2C_SIG_CONN_RSP_PEND) { |
| logd("Connection pending for reason %d\n", status); |
| l2cConnTimer(conn, L2C_L2CAP_OPEN_PEND_TIMEOUT); |
| /* stay in current state until next reply received */ |
| return; |
| } |
| |
| if (result != L2C_SIG_CONN_RSP_OK) { |
| |
| loge("Unexpected result %d for connection to psm %d\n", result, conn->psm); |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to schedule 'closed' state call\n"); |
| l2cConnStructDelete(conn); |
| return; |
| } |
| |
| conn->state = L2C_CONN_STATE_CFG; |
| l2cConnTimer(conn, L2C_L2CAP_CONFIG_TIMEOUT); |
| conn->cfg.cfgTxState = L2C_CFG_STATE_INITIAL; |
| conn->cfg.cfgRxState = L2C_CFG_STATE_INITIAL; |
| conn->remCh = remCh; |
| |
| l2cPsmSendConfig(aclConn, conn); |
| } |
| |
| /* |
| * FUNCTION: l2cPsmSendConfig |
| * USE: RStart a config negotiation with a peer for a new PSM channel |
| * PARAMS: aclConn - the per-ACL-connection structure |
| * conn - the l2c connection struct |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held, when *first* entering config state. May destroy your conn |
| */ |
| static void l2cPsmSendConfig(struct l2cAclConn *aclConn, struct l2cConn *conn) |
| { |
| /* for now our hardwired config is a large MTU */ |
| uint8_t ident; |
| uint8_t config[] = { |
| L2C_CONF_MTU, 0x02, conn->ourMtu & 0xFF, conn->ourMtu >> 8 |
| }; |
| |
| if (conn->cfg.cfgTxState != L2C_CFG_STATE_INITIAL) { |
| loge("Unexpectred call to l2cPsmSendConfig with state %d\n", conn->cfg.cfgTxState); |
| return; |
| } |
| |
| ident = l2cSigSendConfigReq(aclConn, conn->remCh, 0, config, sizeof(config)); |
| if (ident) { |
| conn->ident = ident; |
| conn->cfg.cfgTxState = L2C_CFG_STATE_REQ_SENT; |
| } else { |
| loge("Failed to send config request on conn for chs %d %d on acl "HCI_CONN_FMT"x\n", conn->localCh, conn->remCh, HCI_CONN_CONV(aclConn->conn)); |
| l2cConnRequestClose(aclConn, conn, true); |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cChannelReadyForData |
| * USE: Called when a channel is ready for data to flow |
| * PARAMS: aclConn - the per-ACL-connection structure |
| * conn - the connection struct |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cChannelReadyForData(struct l2cAclConn *aclConn, struct l2cConn *conn) |
| { |
| uint32_t len; |
| uint16_t mtu = conn->theirMtu; |
| |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_OPEN, &conn->handle, sizeof(l2c_handle_t))) { |
| loge("Failed to send open call\n"); |
| l2cConnRequestClose(aclConn, conn, true); |
| return; |
| } |
| |
| /* this failure is non-fatal. they can just ask later */ |
| if (conn->psm && !l2cSvcWorkScheduleStateCall(conn, L2C_STATE_MTU, &mtu, sizeof(mtu))) |
| logw("Failed to notify user of MTU\n"); |
| |
| /* send all queued data, if any */ |
| if (conn->queuedRx) { |
| |
| sg packet; |
| |
| while (sgLength(conn->queuedRx) >= sizeof(uint32_t)) { |
| if (!sgSerializeCutFront(conn->queuedRx, &len, sizeof(len))) { |
| loge("Failed to grab queued packet len\n"); |
| break; |
| } |
| if (sgLength(conn->queuedRx) < len) { |
| loge("Queued packet claims %ub, left %ub\n", len, sgLength(conn->queuedRx)); |
| break; |
| } |
| packet = sgSplit(conn->queuedRx, len); |
| if (!packet) { |
| loge("Failed to split off packet\n"); |
| break; |
| } |
| sgSwap(packet, conn->queuedRx); |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_RX, &packet, sizeof(packet))) { |
| loge("Failed to send queued packet\n"); |
| break; |
| } |
| } |
| if (sgLength(conn->queuedRx)) |
| loge("QueuedRX had leftover data!\n"); |
| sgFree(conn->queuedRx); |
| conn->queuedRx = NULL; |
| } |
| |
| conn->state = L2C_CONN_STATE_ESTABLISHED; |
| } |
| |
| /* |
| * FUNCTION: l2cHandleConfReq |
| * USE: Handle a configuration request for a PSM channel |
| * PARAMS: aclConn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * localCh -local channel number |
| * flags - the flags the remote side sent |
| * payload - the conf payload |
| * len - the length of the abovementioned payload |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleConfReq(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t flags, sg payload, uint16_t len) |
| { |
| struct l2cConn *conn = NULL; |
| uint8_t *reply, *end; |
| uint16_t result = L2C_SIG_CONF_RSP_OK; |
| uint32_t offset = 0; |
| |
| conn = mConns; |
| while (conn && (conn->acl != aclConn->handle || conn->localCh != localCh)) |
| conn = conn->next; |
| |
| if (!conn) { |
| logw("Unexpected Configuration Request id %d for ch %d with result %d\n", ident, localCh, result); |
| if (!l2cSigSendErrInvalidCid(aclConn, ident, localCh, 0)) |
| loge("Failed to send 'invalid cid' reply\n"); |
| return; |
| } |
| |
| if (!conn->psm) { |
| logw("Unexpected Configuration Request for a FixedCh conn chan %d\n", localCh); |
| return; |
| } |
| |
| if (conn->state != L2C_CONN_STATE_CFG) { |
| logw("Configuration Request for connection in bad state %d for ch %d with flags %d\n", conn->state, localCh, flags); |
| return; |
| } |
| |
| if (conn->cfg.cfgRxState != L2C_CFG_STATE_INITIAL) |
| loge("Unexpected internal inconsistency on conf req: expected %d saw %d\n", L2C_CFG_STATE_INITIAL, conn->cfg.cfgRxState); |
| |
| /* go through every option and make up a reply. Note: no reply is longer than request, so this allocation works */ |
| end = reply = (uint8_t*)malloc(len); |
| if (!reply) { |
| loge("Cannot allocate a conf reply\n"); |
| return; |
| } |
| |
| while (len > 2) { |
| uint8_t optType; |
| uint8_t optLen; |
| uint32_t optOfst; |
| |
| sgSerialize(payload, offset++, 1, &optType); |
| sgSerialize(payload, offset++, 1, &optLen); |
| |
| len -= 2; |
| |
| if (optLen > len) { |
| logw("Got invalid conf req option len %d/%d\n", optLen, len); |
| optLen = len; |
| break; |
| } |
| optOfst = offset; |
| offset += optLen; |
| len -= optLen; |
| |
| switch (optType){ |
| case L2C_CONF_MTU: |
| if (optLen != sizeof(uint16_t)) { |
| logi("other side sent bad sized mtu conf: %db\n", optLen); |
| result = L2C_SIG_CONF_RSP_REJECTED; |
| } else { |
| utilSetLE8(end + 0, L2C_CONF_MTU); |
| utilSetLE8(end + 1, sizeof(uint16_t)); |
| sgSerialize(payload, optOfst, sizeof(uint16_t), end + 2); /* copy their mtu */ |
| conn->theirMtu = utilGetLE16(end + 2); |
| end += 4; |
| } |
| break; |
| case L2C_CONF_FLUSH_TIMEOUT: |
| /* we accept whatever the other side says here */ |
| if (optLen != sizeof(uint16_t)) { |
| logi("other side sent bad sized flush to conf: %db\n", optLen); |
| result = L2C_SIG_CONF_RSP_REJECTED; |
| } else { |
| utilSetLE8(end + 0, L2C_CONF_FLUSH_TIMEOUT); |
| utilSetLE8(end + 1, sizeof(uint16_t)); |
| sgSerialize(payload, optOfst, sizeof(uint16_t), end + 2); /* copy the timeout */ |
| end += 4; |
| } |
| break; |
| /* |
| case L2C_CONF_QOS: |
| if (optLen != sizeof(struct l2cConfQosOpts)) { |
| logi("other side sent bad sized qos to conf: %db\n", optLen); |
| result = L2C_SIG_CONF_RSP_REJECTED; |
| } else { |
| struct l2cConfQosOpts *qosOut; |
| |
| // copy over all data first |
| utilSetLE8(end + 0, L2C_CONF_QOS); |
| utilSetLE8(end + 1, sizeof(struct l2cConfQosOpts)); |
| end += 2; |
| qosOut = (struct l2cConfQosOpts*)end; |
| end += sizeof(struct l2cConfQosOpts); |
| sgSerialize(payload, optOfst, sizeof(struct l2cConfQosOpts), qosOut); |
| |
| // change any "required"s to "best effort"s |
| if (utilGetLE8(&qosOut->serviceType) == L2C_QOS_SVC_TYPE_GUARANTEED) |
| utilSetLE8(&qosOut->serviceType, L2C_QOS_SVC_TYPE_BEST_EFFORT); |
| } |
| break; |
| case L2C_CONF_RETR_AND_FLOW: |
| // we do not care what they ask for, we send back basic mode only |
| if (optLen != sizeof(struct l2cConfRetrOpts)) { |
| logi("other side sent bad sized retr to conf: %db\n", optLen); |
| result = L2C_SIG_CONF_RSP_REJECTED; |
| } else { |
| struct l2cConfRetrOpts *retrOut; |
| |
| // copy over all data first |
| utilSetLE8(end + 0, L2C_CONF_RETR_AND_FLOW); |
| utilSetLE8(end + 1, sizeof(struct l2cConfRetrOpts)); |
| end += 2; |
| retrOut = (struct l2cConfRetrOpts*)end; |
| end += sizeof(struct l2cConfRetrOpts); |
| memset(retrOut, 0, sizeof(struct l2cConfRetrOpts)); |
| utilSetLE8(&retrOut->mode, L2C_RETR_MODE_BASIC); |
| } |
| break; |
| case L2C_CONF_FCS: |
| // we do not care what they ask for, we send back "no FCS" only |
| if (optLen != sizeof(suint8_t)) { |
| logi("other side sent bad sized fcs to conf: %db\n", optLen); |
| result = L2C_SIG_CONF_RSP_REJECTED; |
| } else { |
| struct l2cConfRetrOpts *retrOut; |
| utilSetLE8(end + 0, L2C_CONF_FCS); |
| utilSetLE8(end + 1, sizeof(uint8_t)); |
| utilSetLE8(end + 2, 0); |
| end += 3; |
| } |
| break; |
| */ |
| default: |
| logw("Unexpected config data received (t=%d l=%d) ignoring\n", optType, optLen); |
| break; |
| } |
| } |
| |
| if (len) { |
| free(reply); |
| logw("Conf req not processed: %d\n", len); |
| return; |
| } |
| |
| if (!l2cSigSendConfigRsp(aclConn, ident, conn->remCh, flags, result, reply, end - reply)) |
| loge("Failed to sent configuration request response frame\n"); |
| free(reply); |
| |
| if (flags & L2C_SIG_CONF_FLAG_C) |
| logi("Conf Rsp continues\n"); |
| else { |
| conn->cfg.cfgRxState = L2C_CFG_STATE_CFG_ACCEPTED; |
| if (conn->cfg.cfgTxState == L2C_CFG_STATE_CFG_ACCEPTED) { |
| l2cConnTimer(conn, 0); |
| l2cChannelReadyForData(aclConn, conn); |
| } |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cHandleConfRsp |
| * USE: Handle a configuration request for a PSM channel |
| * PARAMS: aclConn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * localCh -local channel number |
| * flags - the flags the remote side sent |
| * result - the result the remote side sent |
| * payload - the payload from the remote side |
| * len - the length of the abovementioned payload |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleConfRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t flags, uint16_t result, sg payload, uint16_t len) |
| { |
| struct l2cConn *conn = NULL; |
| |
| conn = mConns; |
| while (conn && (conn->acl != aclConn->handle || conn->localCh != localCh)) |
| conn = conn->next; |
| |
| if (!conn) { |
| logw("Unexpected Configuration Response id %d for ch %d with result %d\n", ident, localCh, result); |
| return; |
| } |
| |
| if (ident != conn->ident) { |
| logi("RXed a Configuration Response witrh an invalid ident %d (expected %d). Dropping\n", ident, conn->ident); |
| return; |
| } |
| |
| if (!conn->psm) { |
| logw("Unexpected Configuration Response for a FixedCh conn chan %d\n", localCh); |
| return; |
| } |
| |
| if (conn->state != L2C_CONN_STATE_CFG) { |
| logw("Configuration Response for connection in bad state %d %d for ch %d with result %d\n", conn->state, ident, localCh, result); |
| return; |
| } |
| |
| if (result == L2C_SIG_CONF_RSP_UNACCEPTABLE || result == L2C_SIG_CONF_RSP_REJECTED || |
| result == L2C_SIG_CONF_RSP_UNKNOWN_OPTN || result == L2C_SIG_CONF_RSP_PENDING || |
| result == L2C_SIG_CONF_RSP_FLOW_SPEC_REJ) { |
| |
| logi("Other side refused our configuration for psm %d\n", conn->psm); |
| l2cConnRequestClose(aclConn, conn, true); |
| return; |
| } |
| |
| if (result != L2C_SIG_CONF_RSP_OK) { |
| |
| loge("Unexpected result %d for connection to psm %d\n", result, conn->psm); |
| l2cConnRequestClose(aclConn, conn, true); |
| return; |
| } |
| |
| if (conn->cfg.cfgTxState != L2C_CFG_STATE_REQ_SENT) |
| loge("Unexpected internal inconsistency on conf rsp: expected %d saw %d\n", L2C_CFG_STATE_REQ_SENT, conn->cfg.cfgTxState); |
| |
| if (flags & L2C_SIG_CONF_FLAG_C) |
| logi("Conf Rsp continues\n"); |
| else { |
| conn->cfg.cfgTxState = L2C_CFG_STATE_CFG_ACCEPTED; |
| if (conn->cfg.cfgRxState == L2C_CFG_STATE_CFG_ACCEPTED) { |
| l2cChannelReadyForData(aclConn, conn); |
| l2cConnTimer(conn, 0); |
| } |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cHandleInfoReq |
| * USE: Handle an incoming information request |
| * PARAMS: conn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * infoType - the requested info type |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleInfoReq(struct l2cAclConn *aclConn, uint8_t ident, uint16_t infoType) |
| { |
| uint8_t reply[8]; |
| uint32_t infoLen = 0; |
| uint16_t result = L2C_SIG_INFO_RESULT_OK; |
| |
| switch (infoType) { |
| case L2C_SIG_INFO_TYPE_UNCONN_MTU: |
| utilSetLE16(reply, OUR_MTU); |
| infoLen = sizeof(uint16_t); |
| break; |
| case L2C_SIG_INFO_TYPE_EXT_FTRS: |
| utilSetLE32(reply, L2C_EXT_FTRS_SUPPORTED); |
| infoLen = sizeof(uint32_t); |
| break; |
| case L2C_SIG_INFO_TYPE_FIXED_CHS: |
| utilSetLE64(reply, L2C_FIXED_CHS_SUPPORTED); |
| infoLen = sizeof(uint64_t); |
| break; |
| default: |
| result = L2C_SIG_INFO_RESULT_NO; |
| break; |
| } |
| |
| if (!l2cSigSendInfoRsp(aclConn, ident, infoType, result, reply, infoLen)) |
| loge("Failed to sent information request response frame\n"); |
| } |
| |
| /* |
| * FUNCTION: l2cHandleInfoRsp |
| * USE: Handle an incoming information response, enqueue the next one we need, if we need one |
| * PARAMS: conn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * infoType - the requested info type |
| * result - the result of the request |
| * payload - the returned data |
| * infoLen - the returned data's length |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleInfoRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t infoType, uint16_t result, sg payload, uint32_t infoLen) |
| { |
| uint8_t buf[8]; |
| |
| if (aclConn->state != L2C_ACL_STATE_CFG) |
| logw("ACL conn in state %u when RXing info RSPs\n", aclConn->state); |
| |
| if (ident != aclConn->reqIdent) { |
| logw("Got info resp with mismatching ident (got %d wanted %d)\n", ident, aclConn->reqIdent); |
| return; |
| } |
| aclConn->reqIdent = 0; |
| |
| switch (infoType) { |
| case L2C_SIG_INFO_TYPE_UNCONN_MTU: |
| aclConn->connectionlessMtu = 0; |
| if (result == L2C_SIG_INFO_RESULT_OK && sgSerializeCutFront(payload, buf, sizeof(uint16_t))) |
| aclConn->connectionlessMtu = utilGetLE16(buf); |
| else |
| logi("Peer did not properly tell us the connectionless MTU. Asuming not supported\n"); |
| infoType = L2C_SIG_INFO_TYPE_EXT_FTRS; |
| break; |
| case L2C_SIG_INFO_TYPE_EXT_FTRS: |
| aclConn->peerFeatures = 0; |
| if (result == L2C_SIG_INFO_RESULT_OK && sgSerializeCutFront(payload, buf, sizeof(uint32_t))) |
| aclConn->peerFeatures = utilGetLE32(buf); |
| else |
| logi("Peer did not properly tell us supported features. Asuming none.\n"); |
| |
| /* if peer supports fixed channels, find out which */ |
| infoType = (aclConn->peerFeatures & L2C_FTR_FIXED_CHS) ? L2C_SIG_INFO_TYPE_FIXED_CHS : 0; |
| break; |
| case L2C_SIG_INFO_TYPE_FIXED_CHS: |
| aclConn->peerFixedChs = 0; |
| if (result == L2C_SIG_INFO_RESULT_OK && sgSerializeCutFront(payload, buf, sizeof(uint64_t))) |
| aclConn->peerFixedChs = utilGetLE64(buf); |
| else |
| logi("Peer did not properly tell us supported fixed channels. Asuming none.\n"); |
| if (aclConn->state == L2C_ACL_STATE_CFG) |
| aclConn->state = L2C_ACL_STATE_ESTABLISHED; |
| infoType = 0; |
| break; |
| default: |
| logw("Unknown info type 0x%04X RXed. Ignoring\n", infoType); |
| infoType = 0; |
| break; |
| } |
| |
| if (infoType) { |
| aclConn->reqIdent = l2cSigSendInfoReq(aclConn, infoType); |
| if (!aclConn->reqIdent) { |
| aclConn->state = L2C_ACL_STATE_ESTABLISHED; |
| logw("Failed to send req for info %u for acl "HCI_CONN_FMT"x. It will not be known\n", infoType, HCI_CONN_CONV(aclConn->conn)); |
| } |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cHandleEchoReq |
| * USE: Handle an incoming echo request |
| * PARAMS: conn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * payload - the data we got |
| * echoLen - the len we got |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleEchoReq(struct l2cAclConn *aclConn, uint8_t ident, sg payload, uint32_t echoLen) |
| { |
| logi("Got echo packet with %db of data\n", echoLen); |
| |
| /* no data in reply is valid as per spec */ |
| if (!l2cSigSendEchoRsp(aclConn, ident, NULL, 0)) |
| loge("Failed to sent echo response frame\n"); |
| } |
| |
| /* |
| * FUNCTION: l2cHandleConnUpdtReq |
| * USE: Process an incoming connection update request |
| * PARAMS: conn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * intMin - minimum requester interval |
| * intMax - maximum requested interval |
| * latency - requested latency |
| * timeoutMult - requester timeout multiplier |
| * RETURN: num bytes used/needed or 0 for error |
| * NOTES: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleConnUpdtReq(struct l2cAclConn *conn, uint8_t ident, uint16_t intMin, uint16_t intMax, uint16_t latency, uint16_t timeoutMult) |
| { |
| logd("Accepting requested connection update for {%u-%u, %u, %u} with id %u\n", intMin, intMax, latency, timeoutMult, ident); |
| |
| /* verify intervals are in valid range */ |
| if (intMin < BT_LE_CONN_INTERVAL_MIN || intMax < intMin || intMax > BT_LE_CONN_INTERVAL_MAX) |
| goto error; |
| |
| /* verify latency and to params are in valid ranges */ |
| if (latency > BT_LE_LATENCY_MAX || timeoutMult < BT_LE_TIMEOUT_MIN || timeoutMult > BT_LE_TIMEOUT_MAX) |
| goto error; |
| |
| /* verify timout meets BTLE criteria for valid timeout given latency & max interval */ |
| if (timeoutMult < BT_LE_TIMEOUT_MIN_VALID(intMin, latency)) |
| goto error; |
| |
| /* see if we're the master on this cunnection at all */ |
| if (!conn->isMaster) |
| goto error; |
| |
| /* see if another is inflight for this ACL connection and reject if so, else record this one */ |
| if (conn->connUpdtRequestIdent) |
| goto error; |
| conn->connUpdtRequestIdent = ident; |
| |
| /* try to execute it */ |
| if (!l2cSvcWorkScheduleAclConnUpdt(conn->conn, intMin, intMax, latency, timeoutMult)) |
| loge("Failed to schedule connection update work call\n"); |
| else |
| return; |
| |
| error: |
| logw("Unable to request connection update to {[%d, %d], %d, %d}, inflight=%d master=%d\n", |
| intMin, intMax, latency, timeoutMult, ident, conn->isMaster); |
| if (!l2cSigSendConnUpdateRsp(conn, ident, L2C_SIG_CONN_UPDT_NO)) |
| loge("Failed to sent connection update response frame\n"); |
| } |
| |
| /* |
| * FUNCTION: l2cHandleConnUpdtReq |
| * USE: Process an incoming connection update response |
| * PARAMS: conn - the per-ACL-connection structure |
| * ident - the ident in the response packet |
| * result - success or failure of our request |
| * RETURN: num bytes used/needed or 0 for error |
| * NOTES: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cHandleConnUpdtRsp(struct l2cAclConn *conn, uint8_t ident, uint16_t result) |
| { |
| if (conn->isMaster) /* this shouldnt happen if we're master */ |
| loge("Unexpected connection update (ac master) on acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(conn->conn)); |
| else if (!conn->connUpdtRequestIdent) /* if we made no request for this, - error */ |
| loge("Unexpected connection update response on acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(conn->conn)); |
| else { |
| conn->connUpdtRequestIdent = 0; |
| if (conn->connUpdtDoneCbk) { |
| if (!l2cSvcWorkScheduleConnUpdtCbk(conn->connUpdtDoneCbk, conn->connUpdtDoneCbkData, result == L2C_SIG_CONN_UPDT_OK)) |
| loge("Failed to schedule connection update callback\n"); |
| conn->connUpdtDoneCbk = NULL; |
| } |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cApiUpdateConnectionParams |
| * USE: Try to update params on an LE link |
| * PARAMS: handle - the handle |
| * intMin - minimum requester interval |
| * intMax - maximum requested interval |
| * latency - requested latency |
| * timeoutMult - requester timeout multiplier |
| * RETURN: num bytes used/needed or 0 for error |
| * NOTES: NONE |
| */ |
| bool l2cApiUpdateConnectionParams(l2c_handle_t handle, uint16_t intMin, uint16_t intMax, uint16_t latency, uint16_t timeout, l2cConnUpdateDoneCbk doneCbk, void *userData) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false; |
| |
| /* verify intervals are in valid range */ |
| if (intMin < BT_LE_CONN_INTERVAL_MIN || intMax < intMin || intMax > BT_LE_CONN_INTERVAL_MAX) |
| return false; |
| |
| /* verify latency and to params are in valid ranges */ |
| if (latency > BT_LE_LATENCY_MAX || timeout < BT_LE_TIMEOUT_MIN || timeout > BT_LE_TIMEOUT_MAX) |
| return false; |
| |
| /* verify timout meets BTLE criteria for valid timeout given latency & max interval */ |
| if (timeout < BT_LE_TIMEOUT_MIN_VALID(intMin, latency)) |
| return false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Connection not found\n"); |
| goto out; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| goto out; |
| } |
| |
| if (!BT_ADDR_IS_LE(aclConn->peerAddr)) { |
| logw("Got a Connection Update Request API call on a non-LE link\n"); |
| goto out; |
| } |
| |
| if (aclConn->isMaster) { |
| aclConn->connUpdtDoneCbk = doneCbk; |
| aclConn->connUpdtDoneCbkData = userData; |
| ret = l2cSvcWorkScheduleAclConnUpdt(aclConn->conn, intMin, intMax, latency, timeout); |
| if (!ret) |
| loge("Failed to schedule connection update work\n"); |
| } |
| else if (aclConn->connUpdtRequestIdent) |
| logw("Connection update request while another still in progress on acl "HCI_CONN_FMT"x. Ignored\n", HCI_CONN_CONV(aclConn->conn)); |
| else { |
| uint8_t ident; |
| |
| aclConn->connUpdtDoneCbk = doneCbk; |
| aclConn->connUpdtDoneCbkData = userData; |
| |
| ident = l2cSigSendConnUpdateReq(aclConn, intMin, intMax, latency, timeout); |
| if (!ident) |
| loge("Failed to send connection update request for acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| else |
| aclConn->connUpdtRequestIdent = ident; |
| } |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiIsConnEncrypted |
| * USE: Check if connection is encrypted |
| * PARAMS: handle - the handle |
| * RETURN: true if exists and encrypted, false else |
| * NOTES: NONE |
| */ |
| bool l2cApiIsConnEncrypted(l2c_handle_t handle) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Connection not found\n"); |
| goto out; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| goto out; |
| } |
| |
| ret = aclConn->encrypted; |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiIsConnMitmSafe |
| * USE: Check if connection is encrypted using a MITM-safe key |
| * PARAMS: handle - the handle |
| * RETURN: true if exists and encrypted, false else |
| * NOTES: NONE |
| */ |
| bool l2cApiIsConnMitmSafe(l2c_handle_t handle) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Connection not found\n"); |
| goto out; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| goto out; |
| } |
| |
| ret = aclConn->mitmSafe; |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiDemandEncryption |
| * USE: Demand a connection be encrypted |
| * PARAMS: handle - the handle |
| * demandMitmSafe - shoudl it be MITM-safe? |
| * RETURN: true if request taken or already encrypted, false else |
| * NOTES: NONE |
| */ |
| bool l2cApiDemandEncryption(l2c_handle_t handle, bool demandMitmSafe) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Connection not found\n"); |
| goto out; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| goto out; |
| } |
| |
| if (aclConn->encrypted && (aclConn->mitmSafe || !demandMitmSafe)) |
| logd("Ignoring redundant encr request\n"); |
| else if (!l2cSvcWorkScheduleAclDemandEncr(aclConn->conn, demandMitmSafe)) { |
| loge("Failed to demand encryption/authentication by api call\n"); |
| goto out; |
| } |
| |
| ret = true; |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiUserAthorizNotify |
| * USE: Notify other size of successful (or not) user athorization. |
| * PARAMS: handle - the handle |
| * RETURN: true if request taken or already encrypted, false else |
| * NOTES: NONE |
| */ |
| bool l2cApiUserAthorizNotify(l2c_handle_t handle, bool authorized) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| logi("Connection not found for authorization\n"); |
| goto out; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| goto out; |
| } |
| |
| if (conn->state != L2C_CONN_STATE_ATHORIZ_WAIT) { |
| loge("Authorization notification for connection in wrong state\n"); |
| goto out; |
| } |
| |
| if (authorized) { |
| if (conn->psm) { |
| |
| if (!l2cSigSendConnectionRsp(aclConn, conn->ident, conn->localCh, conn->remCh, L2C_SIG_CONN_RSP_OK, 0)) { |
| loge("Failed to sent positive connection request response frame for athoriz - dropping connection\n"); |
| l2cConnRequestClose(aclConn, conn, true); |
| goto out; |
| } |
| |
| logd("config start for PSM %d up after athoriz\n", conn->psm); |
| l2cPsmCfgStart(aclConn, conn); |
| } else { |
| /* handle FixedCh */ |
| |
| logd("Service up for FixedCh %d up after athoriz\n", conn->localCh); |
| l2cChannelReadyForData(aclConn, conn); |
| } |
| } else { |
| logd("Closing conn due to lack of user athorization\n"); |
| l2cConnRequestClose(aclConn, conn, true); |
| } |
| |
| ret = true; |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cHandleDiscReq |
| * USE: Handle a disconnect request from peer |
| * PARAMS: handle - the handle |
| * ident - the ident in the response packet |
| * localCh - local channel number |
| * remCh - remote channel number |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| void l2cHandleDiscReq(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t remCh) |
| { |
| struct l2cConn *conn = NULL; |
| |
| conn = mConns; |
| while (conn && (conn->acl != aclConn->handle || conn->localCh != localCh || conn->remCh != remCh)) |
| conn = conn->next; |
| |
| if (!conn) { |
| logw("Unexpected Disconnection Request id %d for ch %d %d\n", ident, localCh, remCh); |
| if (!l2cSigSendErrInvalidCid(aclConn, ident, localCh, remCh)) |
| loge("Failed to send 'invalid cid' reply\n"); |
| return; |
| } |
| |
| if (!conn->psm) { |
| logw("Unexpected Disconnection Request for a FixedCh conn chan %d %d\n", localCh, remCh); |
| return; |
| } |
| |
| l2cConnRequestClose(aclConn, conn, false); |
| |
| if (!l2cSigSendDiscRsp(aclConn, ident, localCh, remCh)) |
| loge("Failed to sent disconnect request response frame\n"); |
| } |
| |
| /* |
| * FUNCTION: l2cHandleDiscRsp |
| * USE: Handle a disconnect response from peer |
| * PARAMS: handle - the handle |
| * ident - the ident in the response packet |
| * localCh - local channel number |
| * remCh - remote channel number |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| void l2cHandleDiscRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t remCh, uint16_t localCh) |
| { |
| /* We do not care about this, really. We commit to the disconnect whenwe send the request */ |
| logd("Disconnect response from acl "HCI_CONN_FMT"x for ch %d %d\n", HCI_CONN_CONV(aclConn->conn), localCh, remCh); |
| } |
| |
| /* |
| * FUNCTION: l2cSignallingRx |
| * USE: Process arriving data over signalling channels |
| * PARAMS: conn - the per-ACL-connection structure |
| * payload - the signalling packet payload |
| * RETURN: num bytes used/needed or 0 for error |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cSignallingRx(struct l2cAclConn *conn, sg payload) |
| { |
| struct l2cSigFrm sig; |
| |
| while (sgLength(payload) >= sizeof(struct l2cSigFrm)) { |
| uint8_t code, ident; |
| bool cmdParamsSkipped = false; |
| uint16_t len; |
| |
| sgSerialize(payload, 0, sizeof(struct l2cSigFrm), &sig); |
| sgTruncFront(payload, sizeof(struct l2cSigFrm)); |
| |
| code = utilGetLE8(&sig.code); |
| ident = utilGetLE8(&sig.ident); |
| len = utilGetLE16(&sig.len); |
| |
| if (!ident) |
| logw("Got signalling ident 0. Dropping\n"); |
| else switch (code){ |
| case L2C_SIG_CMD_REJ: { |
| struct l2cSigCmdRej params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (len < sizeof(params)) |
| logw("Got a short Command Reject packet\n"); |
| else |
| logw("Got a reject for id %d with reason %d\n", ident, utilGetLE16(¶ms.reason)); |
| break; |
| } |
| case L2C_SIG_CONN_REQ: { |
| struct l2cSigConnReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Connection Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Connection Request packet\n"); |
| else { |
| psm_t psm = utilGetLE16(¶ms.psm); |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| |
| l2cHandleConnReq(conn, ident, psm, scid); |
| } |
| break; |
| } |
| case L2C_SIG_CONN_RSP: { |
| struct l2cSigConnRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Connection Response packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Connection Response packet\n"); |
| else { |
| uint16_t dcid = utilGetLE16(¶ms.dcid); |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| uint16_t result = utilGetLE16(¶ms.result); |
| uint16_t status = utilGetLE16(¶ms.status); |
| |
| l2cHandleConnRsp(conn, ident, dcid, scid, result, status); |
| } |
| break; |
| } |
| case L2C_SIG_CONF_REQ: { |
| struct l2cSigConfReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Configuration Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Configuration Request packet\n"); |
| else { |
| uint16_t dcid = utilGetLE16(¶ms.dcid); |
| uint16_t flags = utilGetLE16(¶ms.flags); |
| |
| sgTruncFront(payload, sizeof(params)); |
| l2cHandleConfReq(conn, ident, dcid, flags, payload, len - sizeof(params)); |
| sgTruncFront(payload, len - sizeof(params)); |
| cmdParamsSkipped = true; |
| } |
| break; |
| } |
| case L2C_SIG_CONF_RSP: { |
| struct l2cSigConfRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Configuration Response packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Configuration Response packet\n"); |
| else { |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| uint16_t flags = utilGetLE16(¶ms.flags); |
| uint16_t result = utilGetLE16(¶ms.result); |
| |
| sgTruncFront(payload, sizeof(params)); |
| l2cHandleConfRsp(conn, ident, scid, flags, result, payload, len - sizeof(params)); |
| sgTruncFront(payload, len - sizeof(params)); |
| cmdParamsSkipped = true; |
| } |
| break; |
| } |
| case L2C_SIG_DISC_REQ: { |
| struct l2cSigDiscReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Disconnect Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Disconnect Request packet\n"); |
| else { |
| uint16_t dcid = utilGetLE16(¶ms.dcid); |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| |
| l2cHandleDiscReq(conn, ident, dcid, scid); |
| } |
| break; |
| } |
| case L2C_SIG_DISC_RSP: { |
| struct l2cSigDiscRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Disconnect Response packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Disconnect Response packet\n"); |
| else { |
| uint16_t dcid = utilGetLE16(¶ms.dcid); |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| |
| l2cHandleDiscRsp(conn, ident, dcid, scid); |
| } |
| break; |
| } |
| case L2C_SIG_ECHO_REQ: { |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Echo Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else |
| l2cHandleEchoReq(conn, ident, payload, len); |
| break; |
| } |
| |
| /* case L2C_SIG_ECHO_RSP: is not needed (we do not ping anyone) */ |
| |
| case L2C_SIG_INFO_REQ: { |
| struct l2cSigInfoReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Info Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Info Request packet\n"); |
| else { |
| uint16_t infoType = utilGetLE16(¶ms.infoType); |
| |
| l2cHandleInfoReq(conn, ident, infoType); |
| } |
| break; |
| } |
| case L2C_SIG_INFO_RSP: { |
| struct l2cSigInfoRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Info Response packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Info Response packet\n"); |
| else { |
| uint16_t infoType = utilGetLE16(¶ms.infoType); |
| uint16_t result = utilGetLE16(¶ms.result); |
| |
| sgTruncFront(payload, sizeof(params)); |
| l2cHandleInfoRsp(conn, ident, infoType, result, payload, len - sizeof(params)); |
| sgTruncFront(payload, len - sizeof(params)); |
| cmdParamsSkipped = true; |
| } |
| break; |
| } |
| /* |
| case L2C_SIG_CH_CREAT_REQ: { |
| struct l2cSigChCreatReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Channel Create Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Channel Create Request packet\n"); |
| else { |
| psm_t psm = utilGetLE16(¶ms.psm); |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| uint8_t controller = utilGetLE8(¶ms.controller); |
| |
| l2cHandleChCreatReq(conn, ident, psm, scid, controller); |
| } |
| break; |
| } |
| case L2C_SIG_CH_CREAT_RSP: { |
| struct l2cSigChCreatRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Channel Create Response packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Channel Create Response packet\n"); |
| else { |
| uint16_t dcid = utilGetLE16(¶ms.dcid); |
| uint16_t scid = utilGetLE16(¶ms.scid); |
| uint16_t result = utilGetLE16(¶ms.result); |
| uint16_t status = utilGetLE16(¶ms.status); |
| |
| l2cHandleChCreatRsp(conn, ident, dcid, scid, result, status); |
| } |
| break; |
| } |
| case L2C_SIG_CH_MOVE_REQ: { |
| struct l2cSigChMoveReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Channel Move Request packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Channel Move Request packet\n"); |
| else { |
| uint16_t icid = utilGetLE16(¶ms.icid); |
| uint8_t controller = utilGetLE8(¶ms.controller); |
| |
| l2cHandleChMoveReq(conn, ident, icid, controller); |
| } |
| break; |
| } |
| case L2C_SIG_CH_MOVE_RSP: { |
| struct l2cSigChMoveRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Channel Move Response packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Channel Move Response packet\n"); |
| else { |
| uint16_t icid = utilGetLE16(¶ms.icid); |
| uint16_t result = utilGetLE8(¶ms.result); |
| |
| l2cHandleChMoveRsp(conn, ident, icid, result); |
| } |
| break; |
| } |
| case L2C_SIG_CH_MOVE_CNF_REQ: { |
| struct l2cSigChMoveCnfReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Channel Move Confirmation packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Channel Move Confirmation packet\n"); |
| else { |
| uint16_t icid = utilGetLE16(¶ms.icid); |
| uint16_t result = utilGetLE8(¶ms.result); |
| |
| l2cHandleChMoveCnfReq(conn, ident, icid, result); |
| } |
| break; |
| } |
| case L2C_SIG_CH_MOVE_CNF_RSP: { |
| struct l2cSigChMoveCnfRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Channel Move Confirmation Response " |
| "packet on a non-EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Channel Move Confirmation Response packet\n"); |
| else { |
| uint16_t icid = utilGetLE16(¶ms.icid); |
| |
| l2cHandleChMoveCnfRsp(conn, ident, icid); |
| } |
| break; |
| } |
| */ |
| case L2C_SIG_CONN_UPDT_REQ: { |
| struct l2cSigConnUpdtReq params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (BT_ADDR_IS_EDR(conn->peerAddr)) { |
| logw("Got a Connection Update Request packet on an EDR link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Connection Update Request packet\n"); |
| else { |
| uint16_t intMin = utilGetLE16(¶ms.intMin); |
| uint16_t intMax = utilGetLE16(¶ms.intMax); |
| uint16_t latency = utilGetLE16(¶ms.latency); |
| uint16_t timeoutMult = utilGetLE16(¶ms.timeoutMult); |
| |
| l2cHandleConnUpdtReq(conn, ident, intMin, intMax, latency, timeoutMult); |
| } |
| break; |
| } |
| case L2C_SIG_CONN_UPDT_RSP: { |
| struct l2cSigConnUpdtRsp params; |
| |
| sgSerialize(payload, 0, sizeof(params), ¶ms); |
| if (!BT_ADDR_IS_LE(conn->peerAddr)) { |
| logw("Got a Connection Update Response packet on a non-LE link\n"); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } else if (len < sizeof(params)) |
| logw("Got a short Connection Update Response packet\n"); |
| else { |
| uint16_t result = utilGetLE8(¶ms.result); |
| |
| l2cHandleConnUpdtRsp(conn, ident, result); |
| } |
| break; |
| } |
| default: { |
| logw("Got unknown code %d on signalling channel\n", code); |
| if (!l2cSigSendErrCmdNotUnderstood(conn, ident)) |
| loge("Failed to send command not understood msg to the other side\n"); |
| } |
| } |
| |
| /* truncate off all the params this command had */ |
| if (!cmdParamsSkipped) |
| sgTruncFront(payload, len); |
| |
| if (!BT_ADDR_IS_EDR(conn->peerAddr)) /* only EDR can put multiple commands here */ |
| break; |
| } |
| |
| if (sgLength(payload)) |
| loge("Leftover %d bytes in signalling frame\n", sgLength(payload)); |
| sgFree(payload); |
| } |
| |
| /* |
| * FUNCTION: l2cAdvTxCreateFragments |
| * USE: Called when we may have space in the buffer list and may want to create a fragment |
| * PARAMS: conn - the conn struct |
| * first - will this be a first fragment? |
| * RETURN: success |
| * NOTES: call with mConnsLock held. |
| */ |
| static bool l2cAdvTxCreateFragments(struct l2cConn *conn, bool first) |
| { |
| bool ret = false; |
| uint16_t len; |
| |
| while (conn->advEnqueuedPacket && !conn->advTxFrags[conn->advTxWritePos].data) { |
| |
| len = sgLength(conn->advEnqueuedPacket); |
| |
| if (len <= conn->advTxMps) { /* fist in one fragment */ |
| if (first) |
| conn->advTxFrags[conn->advTxWritePos].sar = L2C_SAR_COMPLETE; |
| else |
| conn->advTxFrags[conn->advTxWritePos].sar = L2C_SAR_END; |
| conn->advTxFrags[conn->advTxWritePos].data = conn->advEnqueuedPacket; |
| conn->advEnqueuedPacket = NULL; |
| } else { |
| sg data = sgSplit(conn->advEnqueuedPacket, conn->advTxMps - (first ? sizeof(uint16_t) : 0)); |
| |
| if (!data) { |
| loge("Failed to split data sg\n"); |
| return false; |
| } |
| sgSwap(data, conn->advEnqueuedPacket); |
| |
| if (first) { |
| conn->advTxFrags[conn->advTxWritePos].sar = L2C_SAR_START; |
| utilSetLE16(&len, len); |
| if (!sgConcatFrontAlloced(data, &len, sizeof(len))) { |
| loge("Failed to append len. Dropping fragment.\n"); |
| sgFree(data); |
| return false; |
| } |
| } else |
| conn->advTxFrags[conn->advTxWritePos].sar = L2C_SAR_CONT; |
| conn->advTxFrags[conn->advTxWritePos].data = data; |
| } |
| conn->advTxWritePos = (conn->advTxWritePos + 1) % L2C_RETR_NUM_BUFFERS; |
| first = false; |
| ret = true; |
| } |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvUserTx |
| * USE: Called when a user wants us to send data on an advanced connection |
| * PARAMS: conn - the conn struct |
| * data - the data |
| * RETURN: L2C_TX_* |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cAdvUserTx(struct l2cConn *conn, sg data) /* -> L2C_TX_* */ |
| { |
| if (conn->advEnqueuedPacket) /* if a packet is half-sent, we have no space */ |
| return L2C_TX_TRY_LATER; |
| |
| if (conn->advTxFrags[conn->advTxWritePos].data) /* if current write pointer is not empty - we have no space */ |
| return L2C_TX_TRY_LATER; |
| |
| conn->advEnqueuedPacket = data; |
| if (l2cAdvTxCreateFragments(conn, true)) |
| return L2C_TX_ACCEPTED; |
| |
| conn->advEnqueuedPacket = NULL; |
| return L2C_TX_ERROR; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvTxWrapNextPacket |
| * USE: Produce a valid I-frame for a given connection |
| * PARAMS: NONE |
| * RETURN: true if we enqueued a frame |
| * NOTES: call with mConnsLock held. Note that conn is not modified here |
| */ |
| static sg l2cAdvTxWrapNextPacket(struct l2cConn *conn) |
| { |
| uint16_t control = 0, fcs = 0, hdrSz = sizeof(struct l2cFrmHdr) + sizeof(uint16_t); |
| struct l2cAdvFrag *frag = conn->advTxFrags + conn->advTxSendPos; |
| uint8_t buf[sizeof(struct l2cFrmHdr) + sizeof(uint16_t)]; |
| struct l2cFrmHdr *hdr = (struct l2cFrmHdr*)buf; |
| uint16_t *ctrlP = (uint16_t*)(hdr + 1); |
| sg packet = frag->data; |
| |
| if (!packet) |
| return NULL; |
| |
| control |= (uint16_t)conn->advRxFirstMissing << L2C_FLG_SHFT_REQ_SEQ; |
| control |= (uint16_t)conn->advTxSendPos << L2C_FLG_SHFT_TX_SEQ; |
| control |= (uint16_t)frag->sar << L2C_SHFT_SAR; |
| |
| utilSetLE16(&hdr->cid, conn->remCh); |
| utilSetLE16(&hdr->len, sgLength(packet) + hdrSz + (conn->fcs ? sizeof(uint16_t) : 0)); |
| utilSetLE16(ctrlP, control); |
| |
| if (!sgConcatFrontCopy(packet, hdr, hdrSz)) { |
| logw("Failed to append header\n"); |
| return NULL; |
| } |
| |
| if (conn->fcs) { |
| fcs = l2cUtilFcsSg(fcs, packet); |
| utilSetLE16(&fcs, fcs); |
| if (!sgConcatBackCopy(packet, &fcs, sizeof(uint16_t))) { |
| loge("Failed to append FCS\n"); |
| sgTruncFront(packet, hdrSz); |
| return NULL; |
| } |
| } |
| |
| return packet; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvTxUnWrapNextPacket |
| * USE: Remove I-frame headers and footers and put it back into the connection's buffer list. |
| * Essentially undoes what l2cAdvTxWrapNextPacket() did. Only safe to call after a call |
| * to l2cAdvTxWrapNextPacket() and with the struct not modifie din between! |
| * PARAMS: NONE |
| * RETURN: true if we enqueued a frame |
| * NOTES: call with mConnsLock held. Note that conn is not modified here |
| */ |
| static void l2cAdvTxUnWrapNextPacket(struct l2cConn *conn, sg packet) |
| { |
| if (conn->fcs) |
| sgTruncBack(packet, sizeof(uint16_t)); |
| |
| sgTruncFront(packet, sizeof(struct l2cFrmHdr) + sizeof(uint16_t)); |
| } |
| |
| /* |
| * FUNCTION: l2cAdvTxPrepareSFrame |
| * USE: Prepare an S-frame for transmission |
| * PARAMS: conn - the conn struct |
| * RETURN: said frame or NULL |
| * NOTES: call with mConnsLockHeld |
| */ |
| static sg l2cAdvTxPrepareSFrame(struct l2cConn *conn) |
| { |
| const uint16_t l2cPayloadLen = sizeof(uint16_t) + (conn->fcs ? sizeof(uint16_t) : 0); |
| const uint16_t maxPacketLen = sizeof(struct l2cFrmHdr) + sizeof(uint16_t[2]); /* control and fcs */ |
| const uint16_t checksummedLen = sizeof(struct l2cFrmHdr) + sizeof(uint16_t); |
| const uint16_t packetLen = sizeof(struct l2cFrmHdr) + l2cPayloadLen; |
| uint8_t buf[maxPacketLen]; |
| struct l2cFrmHdr *hdr = (struct l2cFrmHdr*)buf; |
| uint16_t *ctrlP = (uint16_t*)(hdr + 1); |
| uint16_t control = L2C_FLG_S_FRM; |
| uint16_t *fcsP = ctrlP + 1; |
| |
| control |= (uint16_t)conn->advRxFirstMissing << L2C_FLG_SHFT_REQ_SEQ; |
| control |= (uint16_t)L2C_S_RR; /* we only do this for now. it i snot necessarily the best idea */ |
| |
| utilSetLE16(&hdr->len, l2cPayloadLen); |
| utilSetLE16(&hdr->cid, conn->remCh); |
| utilSetLE16(ctrlP, control); |
| |
| if (conn->fcs) |
| utilSetLE16(fcsP, l2cUtilFcs(0, buf, checksummedLen)); |
| |
| return sgNewWithCopyData(buf, packetLen); |
| } |
| |
| /* |
| * FUNCTION: l2cAdvMayTxLocked |
| * USE: Called to try to TX on any advanced connection |
| * PARAMS: NONE |
| * RETURN: true if we enqueued a frame |
| * NOTES: call with mConnsLockHeld |
| */ |
| static bool l2cAdvMayTxLocked(void) |
| { |
| struct l2cConn *orig, *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false, isFrmS = false; |
| sg packet; |
| |
| if (!mConns) /* no connections -> we have nothing to send */ |
| return false; |
| |
| orig = mNextToTx; |
| do { |
| if (!mNextToTx) |
| mNextToTx = mConns; |
| conn = mNextToTx; |
| mNextToTx = mNextToTx->next; |
| |
| /* basic connections do not participate */ |
| if (conn->mode == L2C_MODE_BASIC) |
| continue; |
| |
| /* send S-frames first and foremost */ |
| if (conn->advRxWeOweAck) { |
| packet = l2cAdvTxPrepareSFrame(conn); |
| if (!packet) { |
| loge("Failed to produce S frame\n"); |
| continue; |
| } |
| isFrmS = true; |
| } else { |
| |
| /* if the window is full, we cannot send */ |
| if ((conn->advTxSendPos + L2C_RETR_NUM_BUFFERS - conn->advTxFirstUnacked) % L2C_RETR_NUM_BUFFERS >= conn->advTxWindowSz) |
| continue; |
| |
| /* if there is no more data, we cannot send */ |
| if (!conn->advTxFrags[conn->advTxSendPos].data) |
| continue; |
| |
| /* if peer asked us to stop sending data, do so, except s-frames */ |
| if (conn->advTxShutUp) |
| continue; |
| |
| /* if we got here, this connection can send */ |
| packet = l2cAdvTxWrapNextPacket(conn); |
| if (!packet) { |
| logw("Failed to wrap a packet for tx\n"); |
| continue; |
| } |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| logw("Cannot find the ACL link for this connection\n"); |
| continue; |
| } |
| |
| switch (l2cDataSendOnAclLink(aclConn, NULL, 0, packet)) { |
| case L2C_TX_ACCEPTED: |
| conn->advRxWeOweAck = false; |
| if (!isFrmS) { |
| conn->advTxFrags[conn->advTxSendPos++].data = NULL; |
| conn->advTxSendPos %= L2C_RETR_NUM_BUFFERS; |
| } |
| ret = true; |
| break; |
| case L2C_TX_NO_CONN: |
| case L2C_TX_ERROR: |
| logd("ADV TX Failed\n"); |
| sgFree(packet); |
| break; |
| case L2C_TX_TRY_LATER: |
| if (isFrmS) |
| sgFree(packet); |
| else |
| l2cAdvTxUnWrapNextPacket(conn, packet); |
| return false; |
| } |
| |
| } while (mNextToTx != orig); |
| |
| return ret; |
| } |
| |
| |
| /* |
| * FUNCTION: l2cAdvMayTx |
| * USE: Called when TX buffer may have space for more frames |
| * PARAMS: NONE |
| * RETURN: true if we enqueued a frame |
| * NOTES: takes mConnsLock |
| */ |
| static bool l2cAdvMayTx(void) |
| { |
| bool ret; |
| |
| pthread_mutex_lock(&mConnsLock); |
| ret = l2cAdvMayTxLocked(); |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvRxAck |
| * USE: Called when the peer acks some packets |
| * PARAMS: conn - the connection struct |
| * newFirstUnackedPacket - new index of first unacked packet |
| * RETURN: NONE |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static void l2cAdvRxAck(struct l2cConn *conn, uint16_t newFirstUnackedPacket) |
| { |
| uint8_t ackedPackets = (newFirstUnackedPacket + L2C_RETR_NUM_BUFFERS - conn->advTxFirstUnacked) % L2C_RETR_NUM_BUFFERS; |
| bool somethingNewAcked = false; |
| |
| if (ackedPackets > conn->advTxWindowSz) { |
| loge("Peer acked more packets than allowed. Ignoring\n"); |
| return; |
| } |
| |
| while (conn->advTxFirstUnacked != newFirstUnackedPacket) { |
| if (conn->advTxFrags[conn->advTxFirstUnacked].data) |
| sgFree(conn->advTxFrags[conn->advTxFirstUnacked].data); |
| else |
| loge("Peer acked NULL data\n"); |
| conn->advTxFrags[conn->advTxFirstUnacked++].data = NULL; |
| conn->advTxFirstUnacked %= L2C_RETR_NUM_BUFFERS; |
| somethingNewAcked = true; |
| } |
| if (somethingNewAcked && l2cAdvTxCreateFragments(conn, false)) |
| l2cSvcWorkScheduleTryTx(false /*advanced L2C works only on EDR links */); |
| } |
| |
| /* |
| * FUNCTION: l2cAdvEnhFrameRxS |
| * USE: Called to RX an enhanced L2C S-frame with FCS checked |
| * PARAMS: conn - the connection struct |
| * reqSeq - sent reqSeq as per spec |
| * flgF - F flag as per spec |
| * flgP - P flag as per spec |
| * s - sent S value as per spec |
| * RETURN: false if data went nowhere |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static bool l2cAdvEnhFrameRxS(struct l2cConn *conn, uint16_t reqSeq, bool flgF, bool flgP, uint8_t s) |
| { |
| //TODO |
| loge("enhanced mode.s not impl yet\n"); |
| return false; |
| } |
| |
| |
| /* |
| * FUNCTION: l2cAdvEnhFrameRxI |
| * USE: Called to RX an enhanced L2C I-frame with FCS checked |
| * PARAMS: conn - the connection struct |
| * sar - the far flags as per spec |
| * reqSeq - sent reqSeq as per spec |
| * flgF - F flag as per spec |
| * txSeq - sent txSeq as per spec |
| * payload - the actual data |
| * RETURN: false if data went nowhere |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static bool l2cAdvEnhFrameRxI(struct l2cConn *conn, uint8_t sar, uint16_t reqSeq, bool flgF, uint16_t txSeq, sg payload) |
| { |
| //TODO |
| loge("enhanced mode.i not impl yet\n"); |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvFrameRxS |
| * USE: Called to RX a non-enhanced L2C S-frame with FCS checked |
| * PARAMS: conn - the connection struct |
| * reqSeq - sent reqSeq as per spec |
| * flgR - R flag as per spec |
| * s - sent S value as per spec |
| * RETURN: false if data went nowhere |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static bool l2cAdvFrameRxS(struct l2cConn *conn, uint16_t reqSeq, bool flgR, uint8_t s) //XXX: TODO: timers for adv modes! |
| { |
| l2cAdvRxAck(conn, reqSeq); |
| conn->advTxShutUp = flgR; |
| |
| if (s == L2C_S_RR) { |
| //TODO: timers |
| } else if (s == L2C_S_REJ) { // two of these shall never happen again, see "rej exception condition" in spec |
| //TODO: timers |
| conn->advTxSendPos = reqSeq; |
| } else { |
| logw("unexpected S value %d\n", s); |
| return false; |
| } |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvRxReass |
| * USE: Called to attempt to reassemble an incoming fragment using ONE fragment at idx conn->advRxFirstMissing |
| * PARAMS: conn - the connection struct |
| * RETURN: true if we used ONE fragment |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static bool l2cAdvRxReass(struct l2cConn *conn) |
| { |
| struct l2cAdvFrag *frag = conn->advRxFrags + conn->advRxFirstMissing; |
| uint8_t buf[sizeof(uint16_t)]; |
| |
| if (!frag->data) |
| return false; |
| |
| if (conn->advTxReassBuf && (frag->sar == L2C_SAR_START || frag->sar == L2C_SAR_COMPLETE)) { |
| logd("Dropping existing reass buffer because of new packet seen\n"); |
| sgFree(conn->advTxReassBuf); |
| conn->advTxReassBuf = NULL; |
| } |
| |
| if (!conn->advTxReassBuf && (frag->sar == L2C_SAR_CONT || frag->sar == L2C_SAR_END)) { |
| logd("Incosistent RX buf state\n"); |
| return false; |
| } |
| |
| switch (frag->sar) { |
| case L2C_SAR_COMPLETE: |
| conn->advTxReassExpectLen = 0; |
| conn->advTxReassBuf = frag->data; |
| frag->data = NULL; |
| break; |
| case L2C_SAR_START: |
| conn->advTxReassExpectLen = sgLength(frag->data); |
| conn->advTxReassBuf = frag->data; |
| frag->data = NULL; |
| if (!sgSerializeCutFront(conn->advTxReassBuf, buf, sizeof(buf))) { |
| loge("Failed to read length\n"); |
| return false; |
| } |
| conn->advTxReassExpectLen = utilGetLE16(buf); |
| if (conn->advTxReassExpectLen < sgLength(conn->advTxReassBuf)) { |
| loge("Start fragment longer than entire packet\n"); |
| sgFree(conn->advTxReassBuf); |
| conn->advTxReassBuf = NULL; |
| return false; |
| } |
| conn->advTxReassExpectLen -= sgLength(conn->advTxReassBuf); |
| return true; |
| case L2C_SAR_END: |
| case L2C_SAR_CONT: |
| if (sgLength(frag->data) > conn->advTxReassExpectLen) { |
| loge("Non-start fragment longer than expected\n"); |
| return false; |
| } |
| if (sgLength(frag->data) == conn->advTxReassExpectLen && frag->sar == L2C_SAR_CONT) { |
| loge("Non-end fragment ends the expected length\n"); |
| return false; |
| } else if (sgLength(frag->data) != conn->advTxReassExpectLen && frag->sar == L2C_SAR_END) { |
| loge("End fragment does not end the expected length\n"); |
| return false; |
| } |
| conn->advTxReassExpectLen -= sgLength(frag->data); |
| sgConcat(conn->advTxReassBuf, frag->data); |
| frag->data = NULL; |
| break; |
| default: |
| loge("Unknown sar value %u\n", frag->sar); |
| return false; |
| } |
| |
| /* if we got here, we need to send data to the user */ |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_RX, &conn->advTxReassBuf, sizeof(conn->advTxReassBuf))) { |
| loge("Failed to send data to user. Dropped\n"); |
| sgFree(conn->advTxReassBuf); |
| } |
| |
| conn->advTxReassBuf = NULL; |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cAdvFrameRxI |
| * USE: Called to RX a non-enhanced L2C I-frame with FCS checked |
| * PARAMS: conn - the connection struct |
| * sar - the far flags as per spec |
| * reqSeq - sent reqSeq as per spec |
| * flgR - R flag as per spec |
| * txSeq - sent txSeq as per spec |
| * payload - the actual data |
| * RETURN: false if data went nowhere |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static bool l2cAdvFrameRxI(struct l2cConn *conn, uint8_t sar, uint16_t reqSeq, bool flgR, uint16_t txSeq, sg payload) |
| { |
| l2cAdvRxAck(conn, reqSeq); |
| conn->advTxShutUp = flgR; |
| uint8_t numUsed = 0; |
| |
| //TODO: timers |
| |
| if (conn->advRxFrags[txSeq].data) { |
| if (sgLength(conn->advRxFrags[txSeq].data) != sgLength(payload) || conn->advRxFrags[txSeq].sar != sar) { |
| logd("Dropping old fragment of %ub with sar %u in favour of existing one: %ub, sar %u\n", |
| sgLength(conn->advRxFrags[txSeq].data), conn->advRxFrags[txSeq].sar, sgLength(payload), sar); |
| sgFree(conn->advRxFrags[txSeq].data); |
| } else { |
| /* it is a duplicate - move on */ |
| sgFree(payload); |
| return true; |
| } |
| } |
| |
| /* new packet we had not yet seen */ |
| conn->advRxFrags[txSeq].data = payload; |
| conn->advRxFrags[txSeq].sar = sar; |
| |
| while (numUsed < conn->advRxWindowSz && l2cAdvRxReass(conn)) |
| numUsed++; |
| |
| if (numUsed) |
| conn->advRxWeOweAck = true; |
| |
| if (conn->advTxFrags[conn->advTxSendPos].data && !conn->advTxShutUp) |
| l2cAdvMayTxLocked(); |
| |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cHandleCompleteDataFrameRx |
| * USE: Called when given channel data arrives. Dispatches it |
| * to the right destination |
| * PARAMS: aclConn - the ACL connection |
| * frm - the frame header if the packet |
| * payload - the arrived payload |
| * RETURN: false if data went nowhere |
| * NOTES: call with mConnsLock lock held. |
| */ |
| static bool l2cHandleCompleteDataFrameRx(struct l2cAclConn *aclConn, struct l2cFrmHdr *frm, sg payload) |
| { |
| uint16_t cid = utilGetLE16(&frm->cid); |
| struct l2cConn *cur = NULL; |
| bool ret = false; |
| |
| cur = mConns; |
| while (cur) { |
| if (cur->acl == aclConn->handle && cur->localCh == cid) |
| break; |
| cur = cur->next; |
| } |
| |
| if (!cur) { |
| logw("Connection for incoming data on ch %u not found\n", cid); |
| } else if (cur->state == L2C_CONN_STATE_RX_OPEN_SVC && cid < L2C_NUM_FIXED_CHANNELS) { |
| /* Queue the data for later */ |
| |
| uint32_t len = sgLength(payload); |
| if (len + (cur->queuedRx ? sgLength(cur->queuedRx) : 0) > L2C_FIXED_CH_CONN_RX_BACKLOG_MAX) { |
| logw("too much queued for FixedCh connection already. Dropping packet\n"); |
| } else { |
| if (!sgConcatFrontCopy(payload, &len, sizeof(len))) |
| logw("Failed to concat length to queued data. Dropping\n"); |
| else if (cur->queuedRx) |
| sgConcat(cur->queuedRx, payload); |
| else |
| cur->queuedRx = payload; |
| } |
| ret = true; |
| } else if (cur->state != L2C_CONN_STATE_ESTABLISHED) { |
| logw("Connection in state %u on ch %u unable to accept data\n", cur->state, cid); |
| } else { |
| if (!cur->inUse) { |
| logd("Marking conn "HANDLEFMT" as in use due to RX\n", HANDLECNV(cur->handle)); |
| cur->inUse = 1; |
| l2cAclDownTimerStop(aclConn); |
| } |
| if (cur->mode == L2C_MODE_BASIC) |
| ret = l2cSvcWorkScheduleStateCall(cur, L2C_STATE_RX, &payload, sizeof(payload)); |
| else { |
| uint16_t ctrl, fcsGot, fcsCalc, reqSeq; |
| |
| if (cur->fcs) { |
| if (sgLength(payload) < 2 || sizeof(fcsGot) != sgSerialize(payload, sgLength(payload) - sizeof(fcsGot), sizeof(fcsGot), &fcsGot)) { |
| logd("FCS on and cannot read FCS\n"); |
| goto badFrame; |
| } |
| sgTruncBack(payload, sizeof(uint16_t)); |
| fcsGot = utilGetLE16(&fcsGot); |
| fcsCalc = l2cUtilFcs(0, frm, sizeof(struct l2cFrmHdr)); |
| fcsCalc = l2cUtilFcsSg(fcsCalc, payload); |
| |
| if (fcsCalc != fcsGot) { |
| logd("FCS bad. Got 0x%04X Calc 0x%04X\n", fcsGot, fcsCalc); |
| goto badFrame; |
| } |
| } |
| if (!sgSerializeCutFront(payload, &ctrl, sizeof(ctrl))) { |
| logd("Cannot find ctrl\n"); |
| goto badFrame; |
| } |
| reqSeq = (ctrl & L2C_FLG_MASK_REQ_SEQ) >> L2C_FLG_SHFT_REQ_SEQ; |
| if (ctrl & L2C_FLG_S_FRM) { /* S frame */ |
| if (ctrl & L2C_FLG_RSVD_S) |
| goto badFrame; |
| |
| if (cur->mode == L2C_MODE_ENH_RETR) |
| ret = l2cAdvEnhFrameRxS(cur, reqSeq, !!(ctrl & L2C_FLG_F), !!(ctrl & L2C_FLG_P), (ctrl & L2C_MASK_S) >> L2C_SHFT_S); |
| else |
| ret = l2cAdvFrameRxS(cur, reqSeq, !!(ctrl & L2C_FLG_R), (ctrl & L2C_MASK_S) >> L2C_SHFT_S); |
| } else { /* I frame */ |
| uint16_t txSeq = (ctrl & L2C_FLG_MASK_TX_SEQ) >> L2C_FLG_SHFT_TX_SEQ; |
| |
| if (cur->mode == L2C_MODE_ENH_RETR) |
| ret = l2cAdvEnhFrameRxI(cur, (ctrl & L2C_MASK_SAR) >> L2C_SHFT_SAR, reqSeq, !!(ctrl & L2C_FLG_F), txSeq, payload); |
| else |
| ret = l2cAdvFrameRxI(cur, (ctrl & L2C_MASK_SAR) >> L2C_SHFT_SAR, reqSeq, !!(ctrl & L2C_FLG_R), txSeq, payload); |
| } |
| goto out; |
| |
| badFrame: |
| logw("got invalid L2C advanced frame\n"); |
| } |
| } |
| out: |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cAclDataRxCompletePacket |
| * USE: Called when a complete reassembled packet has arrived |
| * PARAMS: aclConn - the acl connection struct |
| * frm - the frame header if the packet |
| * packet - a complete received ACL packet |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cAclDataRxCompletePacket(struct l2cAclConn *aclConn, struct l2cFrmHdr *frm, sg packet) |
| { |
| uint16_t cid = utilGetLE16(&frm->cid); |
| |
| if (cid == L2C_CH_SIGNALLING_EDR) { |
| if (!BT_ADDR_IS_EDR(aclConn->peerAddr)) { |
| loge("Received EDR signalling packet on non-EDR link. Dropping.\n"); |
| sgFree(packet); |
| } else |
| l2cSignallingRx(aclConn, packet); |
| } else if (cid == L2C_CH_SIGNALLING_LE) { |
| if (!BT_ADDR_IS_LE(aclConn->peerAddr)) { |
| loge("Received LE signalling packet on non-LE link. Dropping.\n"); |
| sgFree(packet); |
| } else |
| l2cSignallingRx(aclConn, packet); |
| } else if (cid == L2C_CH_CONNECTIONLESS) { |
| psm_t psm; |
| |
| if (sgLength(packet) < sizeof(psm_t)) { |
| loge("Short connectionless L2CAP data RXed: %db\n", sgLength(packet)); |
| sgFree(packet); |
| } else { |
| sgSerialize(packet, 0, sizeof(psm_t), &psm); |
| sgTruncFront(packet, sizeof(psm_t)); |
| if (!l2cSvcWorkSchedulePsmConnectionlessRx(psm, aclConn->handle, packet)) { |
| loge("Failed to schedule call to connectionless RX for psm %d\n", psm); |
| sgFree(packet); |
| } |
| } |
| } else if (!l2cHandleCompleteDataFrameRx(aclConn, frm, packet)) { |
| if (cid < L2C_NUM_FIXED_CHANNELS) |
| logw("Dropping RXed FixedCh data for cid %d\n", cid); |
| else |
| logw("Dropping RXed PSM data for unknown cid %d\n", cid); |
| sgFree(packet); |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cAclDataRxProcessIfComplete |
| * USE: Called with a [partially] RXed packet to see if it is |
| * complete, and if so process it |
| * PARAMS: conn - the ACL connection struct |
| * packet - the incoming (possibly complete) packet |
| * RETURN: true if packet was complete and was processed |
| * false if more data is needed |
| * NOTES: call with mConnsLock held |
| */ |
| static bool l2cAclDataRxProcessIfComplete(struct l2cAclConn *conn, sg packet) |
| { |
| struct l2cFrmHdr frm; |
| uint16_t frmLen, totalLen; |
| uint32_t len = sgLength(packet); |
| |
| /* must be long enough to fit a header */ |
| if (len < sizeof(struct l2cFrmHdr)) |
| return false; |
| |
| sgSerialize(packet, 0, sizeof(struct l2cFrmHdr), &frm); |
| |
| frmLen = utilGetLE16(&frm.len); |
| totalLen = frmLen + sizeof(struct l2cFrmHdr); |
| |
| /* must fit header and data */ |
| if (totalLen > len) |
| return false; |
| |
| /* throw a warning if extra data */ |
| if (totalLen < len) |
| logw("ACL data packet %ub but L2C expects %ub\n", len, totalLen); |
| |
| /* full packet - process it */ |
| sgTruncFront(packet, sizeof(struct l2cFrmHdr)); |
| l2cAclDataRxCompletePacket(conn, &frm, packet); |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cAclDataRx |
| * USE: Called by lower layers when ACL data arrives |
| * PARAMS: aclConnID - the connection ID |
| * packet - the incoming packet |
| * first - whethere this is first or continued packet |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cAclDataRx(hci_conn_t aclConnID, sg packet, bool first) |
| { |
| struct l2cAclConn *conn; |
| uint32_t datalen = sgLength(packet); |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cAclConnFindById(aclConnID); |
| if (conn) { |
| if (!first && !conn->reassSg) |
| logw("Got %ub non-first fragment while no reassemble buffer. Dropping\n", datalen); |
| else { |
| if (first) { |
| if (conn->reassSg) { |
| logw("Got a first packet while reassemble buffer has %ub. Dropping previous fragment\n", sgLength(conn->reassSg)); |
| sgFree(conn->reassSg); |
| conn->reassSg = NULL; |
| } |
| if (!l2cAclDataRxProcessIfComplete(conn, packet)) { |
| conn->reassSg = packet; |
| } |
| packet = NULL; |
| } else if (!conn->reassSg) |
| logw("Got non-first packet when reassembly buffer empty. Dropping %ud\n", datalen); |
| else { |
| sgConcat(conn->reassSg, packet); |
| packet = NULL; |
| |
| if (l2cAclDataRxProcessIfComplete(conn, conn->reassSg)) |
| conn->reassSg = NULL; |
| } |
| } |
| } else |
| loge("Unable to find acl "HCI_CONN_FMT"x. Dropping packet\n", HCI_CONN_CONV(aclConnID)); |
| |
| if (packet) |
| sgFree(packet); |
| |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cPsmSvcFindByPsm |
| * USE: Find a service structure by PSM |
| * PARAMS: psm - the psm |
| * RETURN: service structure or NULL if not found |
| * NOTES: Must hold the mSvcsLock lock while calling this |
| */ |
| static struct l2cPsmSvc* l2cPsmSvcFindByPsm(psm_t psm) |
| { |
| struct l2cPsmSvc* svc = mPsmSvcs; |
| |
| while (svc && svc->psm != psm) |
| svc = svc->next; |
| |
| return svc; |
| } |
| /* |
| * FUNCTION: l2cFixedChSvcFindByChan |
| * USE: Find a service structure by fixed channel |
| * PARAMS: chan - the channel |
| * RETURN: service structure or NULL if not found |
| * NOTES: Must hold the mSvcsLock lock while calling this |
| */ |
| static struct l2cFixedChSvc* l2cFixedChSvcFindByChan(uint16_t chan) |
| { |
| struct l2cFixedChSvc* svc = mFixedSvcs; |
| |
| while (svc && svc->chan != chan) |
| svc = svc->next; |
| |
| return svc; |
| } |
| |
| /* |
| * FUNCTION: l2cCommonSvcInstAllocDone |
| * USE: Called when a service call to allocate a PSM of a FixedCh service instance finishes |
| * PARAMS: handle - the handle param for the connection |
| * connP - where to put pointer to per-conn struct |
| * aclConnP - where to put pointer to per-ACL-link struct |
| * RETURN: true if all was successful, false else |
| * NOTES: call with mConnsLock held |
| */ |
| static bool l2cCommonSvcInstAllocDone(l2c_handle_t handle, struct l2cConn **connP, struct l2cAclConn **aclConnP) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Connection not found\n"); |
| return false; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| return false; |
| } |
| |
| *connP = conn; |
| *aclConnP = aclConn; |
| |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cPsmCfgStart |
| * USE: Start config (once channel open reply has been sent) |
| * PARAMS: aclConn - the acl connection struct |
| * conn - the l2c connection struct |
| * RETURN: NONE |
| * NOTES: call with mConnsLock held |
| */ |
| static void l2cPsmCfgStart(struct l2cAclConn *aclConn, struct l2cConn *conn) |
| { |
| conn->state = L2C_CONN_STATE_CFG; |
| l2cConnTimer(conn, L2C_L2CAP_CONFIG_TIMEOUT); |
| conn->cfg.cfgTxState = L2C_CFG_STATE_INITIAL; |
| conn->cfg.cfgRxState = L2C_CFG_STATE_INITIAL; |
| l2cPsmSendConfig(aclConn, conn); |
| } |
| |
| /* |
| * FUNCTION: l2cPsmSvcInstAllocDone |
| * USE: Called when a service call to allocate a PSM service instance finishes |
| * PARAMS: l2cStateCbk - the state cbk for this conn |
| * userData - param for state callbacks |
| * result - SVC_ALLOC_* from service instance allocate callback |
| * instance - the allocated instance or NULL |
| * handle - the handle param for the connection |
| * RETURN: NONE |
| * NOTES: May destroy your conn |
| */ |
| static void l2cPsmSvcInstAllocDone(psm_t psm, l2cStateCbk stateCbk, void *userData, uint8_t result, void *instance, l2c_handle_t handle) |
| { |
| struct l2cConn* conn; |
| struct l2cAclConn* aclConn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| if (!l2cCommonSvcInstAllocDone(handle, &conn, &aclConn)) |
| goto out; |
| |
| if (result == SVC_ALLOC_SUCCESS && conn->state == L2C_CONN_STATE_TEARDOWN) { |
| if (!l2cSvcWorkScheduleStateCallDirect(stateCbk, userData, instance, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to close PSM after teardown request in alloc done handler\n"); |
| /* this is the closes error code to what we want here */ |
| if (!l2cSigSendConnectionRsp(aclConn, conn->ident, 0, conn->remCh, L2C_SIG_CONN_RSP_NO_RSRC, 0)) |
| loge("Failed to sent negative connection request response frame\n"); |
| l2cConnStructDelete(conn); |
| goto out; |
| } |
| |
| if (result == SVC_ALLOC_DEMAND_ENCR || result == SVC_ALLOC_DEMAND_AUTH) { |
| if (!l2cSigSendConnectionRsp(aclConn, conn->ident, 0, conn->remCh, L2C_SIG_CONN_RSP_PEND, L2C_SIG_CONN_PEND_AUTH)) { |
| loge("Failed to sent pending.auth connection request response frame\n"); |
| l2cConnRequestClose(aclConn, conn, false); |
| goto out; |
| } |
| conn->state = L2C_CONN_STATE_ENCR_WAIT; |
| if (aclConn->encrypted && (aclConn->mitmSafe || result != SVC_ALLOC_DEMAND_AUTH)) { |
| logi("Link acl "HCI_CONN_FMT"x already encrypted to the requested level\n", HCI_CONN_CONV(aclConn->conn)); |
| if (!l2cSvcWorkSchedulePsmAlloc(psm, handle)) |
| loge("Failed to re-enquque PSM alloc\n"); |
| } else { |
| if (!l2cSvcWorkScheduleAclDemandEncr(aclConn->conn, result == SVC_ALLOC_DEMAND_AUTH)) |
| loge("Failed to demand encr/auth\n"); |
| logd("Demanded encr/auth on acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| } |
| goto out; |
| } |
| |
| if (result != SVC_ALLOC_SUCCESS && result != SVC_ALLOC_WAIT_FOR_AUTHORIZ) { |
| uint16_t retCode = L2C_SIG_CONN_RSP_NO_RSRC; |
| |
| if (result == SVC_ALLOC_FAIL_SECURITY) |
| retCode = L2C_SIG_CONN_RSP_BAD_SEC; |
| else if (result == SVC_ALLOC_FAIL_BEING_REMOVED || result == SVC_ALLOC_FAIL_NO_CONNS) |
| retCode = L2C_SIG_CONN_RSP_BAD_PSM; |
| |
| logi("Service allocations failed for psm %d with result %d code %d\n", conn->psm, result, retCode); |
| if (!l2cSigSendConnectionRsp(aclConn, conn->ident, 0, conn->remCh, retCode, 0)) |
| loge("Failed to sent negative connection request response frame\n"); |
| logd("Dropping connection struct\n"); |
| l2cConnStructDelete(conn); |
| goto out; |
| } |
| |
| conn->stateCbk = stateCbk; |
| conn->userData = userData; |
| conn->instance = instance; |
| |
| if (result == SVC_ALLOC_WAIT_FOR_AUTHORIZ) { |
| if (!l2cSigSendConnectionRsp(aclConn, conn->ident, 0, conn->remCh, L2C_SIG_CONN_RSP_PEND, L2C_SIG_CONN_PEND_ATHORIZ)) { |
| loge("Failed to sent pending.athoriz connection request response frame\n"); |
| l2cConnRequestClose(aclConn, conn, true); |
| goto out; |
| } |
| conn->state = L2C_CONN_STATE_ATHORIZ_WAIT; |
| goto out; |
| } |
| |
| if (!l2cSigSendConnectionRsp(aclConn, conn->ident, conn->localCh, conn->remCh, L2C_SIG_CONN_RSP_OK, 0)) { |
| loge("Failed to sent positive connection request response frame - dropping connection\n"); |
| l2cConnRequestClose(aclConn, conn, false); |
| goto out; |
| } |
| |
| l2cPsmCfgStart(aclConn, conn); |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cFixedChSvcInstAllocDone |
| * USE: Called when a service call to allocate a FixedCh service instance finishes |
| * PARAMS: chan - the channel originally requested |
| * l2cStateCbk - the state cbk for this conn |
| * userData - param for state callbacks |
| * result - SVC_ALLOC_* from service instance allocate callback |
| * instance - the allocated instance or NULL |
| * handle - the handle param for the connection |
| * RETURN: NONE |
| * NOTES: |
| */ |
| static void l2cFixedChSvcInstAllocDone(uint16_t chan, l2cStateCbk stateCbk, void *userData, uint8_t result, void *instance, l2c_handle_t handle) |
| { |
| struct l2cConn* conn; |
| struct l2cAclConn* aclConn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| if (!l2cCommonSvcInstAllocDone(handle, &conn, &aclConn)) |
| goto out; |
| |
| if (result == SVC_ALLOC_SUCCESS && conn->state == L2C_CONN_STATE_TEARDOWN) { |
| if (!l2cSvcWorkScheduleStateCallDirect(stateCbk, userData, instance, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to close FixedCh after teardown request in alloc done handler\n"); |
| l2cConnStructDelete(conn); |
| goto out; |
| } |
| |
| if (conn->state != L2C_CONN_STATE_RX_OPEN_SVC) |
| logw("Unexpected FixedCh conn state %d after alloc\n", conn->state); |
| |
| if (result == SVC_ALLOC_DEMAND_ENCR || result == SVC_ALLOC_DEMAND_AUTH) { |
| if (aclConn->encrypted && (aclConn->mitmSafe || result != SVC_ALLOC_DEMAND_AUTH)) { |
| logi("Link acl "HCI_CONN_FMT"x already encrypted to the requested level\n", HCI_CONN_CONV(aclConn->conn)); |
| if (!l2cSvcWorkScheduleFixedChAlloc(chan, handle)) { |
| loge("Failed to re-enqueue fixed alloc\n"); |
| l2cConnRequestClose(aclConn, conn, false); |
| goto out; |
| } |
| } else { |
| if (!l2cSvcWorkScheduleAclDemandEncr(aclConn->conn, result == SVC_ALLOC_DEMAND_AUTH)) { |
| loge("Failed to demand encryption\n"); |
| l2cConnRequestClose(aclConn, conn, false); |
| goto out; |
| } |
| logd("Demanded encryption on acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| conn->state = L2C_CONN_STATE_ENCR_WAIT; |
| } |
| goto out; |
| } |
| |
| if (result != SVC_ALLOC_SUCCESS && result != SVC_ALLOC_WAIT_FOR_AUTHORIZ) { |
| logi("Service allocations failed for FixedCh %d\n", conn->localCh); |
| logd("Dropping connection struct\n"); |
| l2cConnStructDelete(conn); |
| goto out; |
| } |
| |
| conn->stateCbk = stateCbk; |
| conn->userData = userData; |
| conn->instance = instance; |
| |
| if (result == SVC_ALLOC_WAIT_FOR_AUTHORIZ) { |
| conn->state = L2C_CONN_STATE_ATHORIZ_WAIT; |
| goto out; |
| } |
| |
| logd("Service up for FixedCh %d\n", conn->localCh); |
| l2cChannelReadyForData(aclConn, conn); |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cSvcWorker |
| * USE: Worker thread to call up to services |
| * PARAMS: unused - not used |
| * RETURN: NONE |
| * NOTES: Used to call services and not get blocked by them. |
| */ |
| static void* l2cSvcWorker(void* unused) |
| { |
| struct l2cSvcWork *work; |
| struct l2cPsmSvc *psmSvc; |
| struct l2cFixedChSvc *fixedSvc; |
| psm_t psm; |
| uint16_t chan; |
| int status; |
| |
| pthread_setname_np(pthread_self(), "bt_l2cap_worker"); |
| |
| while(1) { |
| void *instance = NULL; |
| uint8_t result; |
| |
| status = workQueueGet(mServiceWork, (void**)&work); |
| |
| /* The only one thing we use the work queue status for - exiting */ |
| if (status) |
| break; |
| |
| switch (work->type) { |
| case L2C_WORK_PSM_INST_ALLOC: |
| psm = work->i_alloc.which.psm; |
| pthread_mutex_lock(&mSvcsLock); |
| psmSvc = l2cPsmSvcFindByPsm(psm); |
| if (!psmSvc) { |
| logw("Got work for a not-found PSM-based service %d\n", psm); |
| result = SVC_ALLOC_FAIL_OTHER; |
| } else if (psmSvc->beingRemoved) { |
| logi("Ignoring instance alloc for service (psm=%d) being removed\n", psm); |
| result = SVC_ALLOC_FAIL_BEING_REMOVED; |
| } else if (!psmSvc->desc.serviceInstanceAlloc) { |
| logi("Ignoring instance alloc for service (psm=%d) not supporting connections\n", psm); |
| result = SVC_ALLOC_FAIL_NO_CONNS; |
| } else { |
| result = psmSvc->desc.serviceInstanceAlloc(psmSvc->desc.userData, work->i_alloc.handle, &instance); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| l2cPsmSvcInstAllocDone(psm, psmSvc->desc.serviceInstanceStateCbk, psmSvc->desc.userData, result, instance, work->i_alloc.handle); |
| break; |
| case L2C_WORK_PSM_CONNECTIONLESS_RX: |
| psm = work->cl_rx.psm; |
| pthread_mutex_lock(&mSvcsLock); |
| psmSvc = l2cPsmSvcFindByPsm(psm); |
| if (!psmSvc) { |
| logw("Got work for a not-found PSM-based service %d\n", psm); |
| } else { |
| if (psmSvc->beingRemoved) { |
| logi("Ignoring connectionless rx for service (psm=%d) being removed\n", psm); |
| } else { |
| if (psmSvc->desc.serviceConnectionlessRx) { |
| psmSvc->desc.serviceConnectionlessRx(psmSvc->desc.userData, work->cl_rx.handle, work->cl_rx.payload); |
| work->cl_rx.payload = NULL; |
| } |
| } |
| if (work->cl_rx.payload) |
| sgFree(work->cl_rx.payload); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| break; |
| case L2C_WORK_PSM_UNREGISTER: |
| psm = work->unreg.which.psm; |
| pthread_mutex_lock(&mSvcsLock); |
| psmSvc = l2cPsmSvcFindByPsm(psm); |
| if (!psmSvc) { |
| logw("Got work for a not-found PSM-based service %d\n", psm); |
| } else { |
| if (!psmSvc->beingRemoved) |
| logw("Unregistering a service (psm=%d) unexpectedly\n", psm); |
| if (psmSvc->next) |
| psmSvc->next->prev = psmSvc->prev; |
| if (psmSvc->prev) |
| psmSvc->prev->next = psmSvc->next; |
| else |
| mPsmSvcs = psmSvc->next; |
| free(psmSvc); |
| if (work->unreg.doneCbk) |
| work->unreg.doneCbk(work->unreg.cbkData); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| break; |
| case L2C_WORK_FIXEDCH_INST_ALLOC: |
| chan = work->i_alloc.which.chan; |
| pthread_mutex_lock(&mSvcsLock); |
| fixedSvc = l2cFixedChSvcFindByChan(chan); |
| if (!fixedSvc) { |
| logw("Got work for a not-found FixedCh service %d\n", chan); |
| result = SVC_ALLOC_FAIL_OTHER; |
| } else if (fixedSvc->beingRemoved) { |
| logi("Ignoring instance alloc for service (chan=%d) being removed\n", chan); |
| result = SVC_ALLOC_FAIL_BEING_REMOVED; |
| } else { |
| result = fixedSvc->desc.serviceFixedChAlloc(fixedSvc->desc.userData, work->i_alloc.handle, &instance); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| l2cFixedChSvcInstAllocDone(chan, fixedSvc->desc.serviceFixedChStateCbk, fixedSvc->desc.userData, result, instance, work->i_alloc.handle); |
| break; |
| case L2C_WORK_FIXEDCH_UNREGISTER: |
| chan = work->unreg.which.chan; |
| pthread_mutex_lock(&mSvcsLock); |
| fixedSvc = l2cFixedChSvcFindByChan(chan); |
| if (!fixedSvc) { |
| logw("Got work for a not-found FixedCh service %d\n", chan); |
| } else { |
| if (!fixedSvc->beingRemoved) |
| logw("Unregistering a service (chan=%d) unexpectedly\n", chan); |
| if (fixedSvc->next) |
| fixedSvc->next->prev = fixedSvc->prev; |
| if (fixedSvc->prev) |
| fixedSvc->prev->next = fixedSvc->next; |
| else |
| mFixedSvcs = fixedSvc->next; |
| free(fixedSvc); |
| if (work->unreg.doneCbk) |
| work->unreg.doneCbk(work->unreg.cbkData); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| break; |
| case L2C_WORK_STATE_CBK: |
| work->state.stateCbk(work->state.userData, work->state.instance, work->state.state, work + 1, work->state.len); |
| break; |
| case L2C_WORK_CONN_UPDT_CBK: |
| work->connUpdtCbk.doneCbk(work->connUpdtCbk.cbkData, work->connUpdtCbk.success); |
| break; |
| case L2C_WORK_ACL_LINK_OPEN: |
| //todo: non-default settings? |
| if (!hciConnect(&work->linkOpen.addr, NULL, NULL, NULL, NULL, NULL, NULL, NULL)) |
| loge("HCI refused to connect\n"); |
| break; |
| case L2C_WORK_ACL_LINK_CLOSE: |
| if (!hciDisconnect(work->linkClose.aclConn)) |
| loge("HCI refused to disconnect\n"); |
| break; |
| case L2C_WORK_ACL_CONN_UPDT: |
| if (!hciUpdateLeParams(work->connUpdt.aclConnID, work->connUpdt.minInt, work->connUpdt.maxInt, work->connUpdt.lat, work->connUpdt.to)) |
| loge("HCI refused to update LE params\n"); |
| break; |
| case L2C_WORK_ACL_DEMAND_ENCR: |
| if (!hciDemandEncr(work->demandEncr.aclConnID, work->demandEncr.demandMitmSafe)) |
| loge("HCI refused to encrypt\n"); |
| break; |
| case L2C_WORK_TRY_HCI_TX: |
| l2cTryMakeSendProgress(work->tryTx.useLeBuffers); |
| break; |
| case L2C_WORK_DEFAULT_SM_CALL: |
| work->defaultSm.cbk(work->defaultSm.hciConn, work->defaultSm.randomNum, work->defaultSm.diversifier); |
| break; |
| case L2C_WORK_FLUSH: |
| sem_post(work->flush.sem); |
| break; |
| default: { |
| loge("Unknown work type %d\n", work->type); |
| break; |
| } |
| } |
| free(work); |
| } |
| |
| return NULL; |
| } |
| |
| /* |
| * FUNCTION: l2cAclEnqueueSigTx |
| * USE: Enqueue a signalling PDU |
| * PARAMS: aclConn - the ACL connection structure |
| * pdu - the PDU data |
| * RETURN: true is a send was requested. False on immediate error. |
| * NOTES: Call only with mConnsLock held. |
| * ONLY FOR SIGNALLING FRAMES. WILL NOT BLOCK. |
| */ |
| static bool l2cAclEnqueueSigTx(struct l2cAclConn *aclConn, sg pdu) |
| { |
| bool isLE = BT_ADDR_IS_LE(aclConn->peerAddr); |
| bool useLeBuffers = isLE && !mChipJointBuffers; |
| struct l2cSigPduSendItem **qH = isLE ? &sigSendHeadLe : &sigSendHeadEdr; |
| struct l2cSigPduSendItem **qT = isLE ? &sigSendTailLe : &sigSendTailEdr; |
| struct l2cSigPduSendItem *item; |
| uint16_t len = sgLength(pdu); |
| |
| if (len > aclConn->sigMtu) { |
| loge("Rejecting L2C SIG TX over MTU len: %u > %u\n", len, aclConn->sigMtu); |
| return false; |
| } |
| |
| /* if not LE, we try to merge. Find last SIG frame to same connection and see if we can merge with it. */ |
| if (!isLE) { |
| struct l2cSigPduSendItem *i = *qT; |
| while (i && i->aclConnID != aclConn->conn) |
| i = i->prev; |
| if (i && !i->hasL2cHdr && sgLength(i->pdu) + len <= aclConn->sigMtu) { |
| sgConcat(i->pdu, pdu); |
| return true; |
| } |
| } |
| |
| item = (struct l2cSigPduSendItem*)calloc(1, sizeof(struct l2cSigPduSendItem)); |
| if (!item) |
| return false; |
| |
| item->aclConnID = aclConn->conn; |
| item->pdu = pdu; |
| item->next = NULL; |
| item->prev = (*qT); |
| item->cid = BT_ADDR_IS_LE(aclConn->peerAddr) ? L2C_CH_SIGNALLING_LE : L2C_CH_SIGNALLING_EDR; |
| item->hasL2cHdr = false; |
| if (item->prev) |
| item->prev->next = item; |
| else |
| (*qH) = item; |
| *qT = item; |
| |
| if (!l2cSvcWorkScheduleTryTx(useLeBuffers)) |
| loge("Failed to schedule TX attempt\n"); |
| |
| return true; |
| } |
| |
| /* |
| * FUNCTION: l2cWorkFreeSigTx |
| * USE: Called to cleanup each item of signalling TX queue at deinit time |
| * PARAMS: workItem - a work item |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cWorkFreeSigTx(void *workItem) |
| { |
| struct l2cSigFrameWork *work = (struct l2cSigFrameWork*)workItem; |
| |
| free(work->data); |
| free(work); |
| } |
| |
| /* |
| * FUNCTION: l2cWorkFreeService |
| * USE: Called to cleanup each item of work queue at deinit time |
| * PARAMS: workItem - a work item |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cWorkFreeService(void *workItem) |
| { |
| struct l2cSvcWork *work = (struct l2cSvcWork*)workItem; |
| |
| switch (work->type) { |
| case L2C_WORK_PSM_CONNECTIONLESS_RX: |
| sgFree(work->cl_rx.payload); |
| break; |
| } |
| free(work); |
| } |
| |
| /* |
| * FUNCTION: l2cInit |
| * USE: Called to init of L2CAP state |
| * PARAMS: NONE |
| * RETURN: NONE |
| * NOTES: Initializes the l2cap layer |
| */ |
| int l2cInit(void) |
| { |
| int ret; |
| |
| mNextPsm = 0x1001; |
| |
| mQueueFirstLe = true; |
| mQueueFirstEdr = true; |
| mQueueConnLe = 0; |
| mQueueConnEdr = 0; |
| |
| mServiceWork = workQueueAlloc(L2C_SVC_NUM_OUTSTANDING_REQS); |
| if (!mServiceWork){ |
| loge("Failed to allocate service workqueue\n"); |
| ret = ENOMEM; |
| goto out_service_q; |
| } |
| |
| ret = pthread_create(&mSvcWorker, NULL, l2cSvcWorker, NULL); |
| if (ret) { |
| loge("Failed(%d) to create service worker\n", ret); |
| goto out_service_thread; |
| } |
| |
| mWriteNotif = multiNotifCreate(); |
| if (!mWriteNotif) { |
| loge("Failed to create write notif list\n"); |
| goto out_multinotif; |
| } |
| |
| mChipJointBuffers = hciInfoSharedBuffers(); |
| if (!hciInfoAclBufSizeLe(&mChipBufLenLe, &mChipBufNumLe)) |
| logi("No LE support found\n"); |
| |
| return 0; |
| |
| out_multinotif: |
| workQueueWakeAll(mServiceWork, 1); |
| pthread_join(mSvcWorker, NULL); |
| |
| out_service_thread: |
| workQueueFree(mServiceWork, l2cWorkFreeService); |
| |
| out_service_q: |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cDeinit |
| * USE: Unilaterally (and quickly) close l2cap |
| * PARAMS: NONE |
| * RETURN: NONE |
| * NOTES: the other side never gets to know what hit it |
| */ |
| void l2cDeinit(void) |
| { |
| pthread_mutex_lock(&mConnsLock); |
| while (mAclConns) |
| l2cAclLinkDownInt(mAclConns, false); |
| if (mConns) |
| logw("Connections list not empty even when all acl links closed\n"); |
| mConns = NULL; |
| mNextToTx = NULL; |
| pthread_mutex_unlock(&mConnsLock); |
| while (mPsmSvcs) |
| l2cApiServicePsmUnregister(mPsmSvcs->psm, NULL, NULL); |
| while (mFixedSvcs) |
| l2cApiServiceFixedChUnregister(mFixedSvcs->chan, NULL, NULL); |
| |
| if (mQueueBufLe) |
| sgFree(mQueueBufLe); |
| |
| if (mQueueBufEdr) |
| sgFree(mQueueBufEdr); |
| |
| mQueueBufLe = NULL; |
| mQueueBufEdr = NULL; |
| |
| while (sigSendHeadEdr) { |
| sigSendTailEdr = sigSendHeadEdr; |
| sigSendHeadEdr = sigSendHeadEdr->next; |
| sgFree(sigSendTailEdr->pdu); |
| free(sigSendTailEdr); |
| } |
| sigSendTailEdr = NULL; |
| |
| while (sigSendHeadLe) { |
| sigSendTailLe = sigSendHeadEdr; |
| sigSendHeadLe = sigSendHeadLe->next; |
| sgFree(sigSendTailLe->pdu); |
| free(sigSendTailLe); |
| } |
| sigSendTailLe = NULL; |
| |
| multiNotifDestroy(mWriteNotif); |
| workQueueWakeAll(mServiceWork, 1); |
| pthread_join(mSvcWorker, NULL); |
| workQueueFree(mServiceWork, l2cWorkFreeService); |
| } |
| |
| |
| /* |
| * FUNCTION: l2cConnStructDelete |
| * USE: Delete an l2cConn structure |
| * PARAMS: conn - the connection structure |
| * RETURN: NONE |
| * NOTES: Call with mConnsLock held. |
| */ |
| static void l2cConnStructDelete(struct l2cConn *conn) |
| { |
| uint8_t i; |
| |
| if (mNextToTx == conn) |
| mNextToTx = NULL; |
| |
| if (conn->queuedRx) { |
| sgFree(conn->queuedRx); |
| conn->queuedRx = NULL; |
| } |
| |
| if (conn->next) |
| conn->next->prev = conn->prev; |
| if (conn->prev) |
| conn->prev->next = conn->next; |
| else |
| mConns = conn->next; |
| |
| for (i = 0; i < L2C_RETR_NUM_BUFFERS; i++) { |
| if (conn->advTxFrags[i].data) |
| sgFree(conn->advTxFrags[i].data); |
| conn->advTxFrags[i].data = NULL; |
| if (conn->advRxFrags[i].data) |
| sgFree(conn->advRxFrags[i].data); |
| conn->advRxFrags[i].data = NULL; |
| } |
| if (conn->advEnqueuedPacket) |
| sgFree(conn->advEnqueuedPacket); |
| conn->advEnqueuedPacket = NULL; |
| |
| if (conn->advTxReassBuf) |
| sgFree(conn->advTxReassBuf); |
| conn->advTxReassBuf = NULL; |
| |
| free(conn); |
| } |
| |
| /* |
| * FUNCTION: l2cAclConnStructDelete |
| * USE: Delete an l2cAclConn structure |
| * PARAMS: conn - the connection structure |
| * RETURN: NONE |
| * NOTES: Call with mConnsLock held. |
| */ |
| static void l2cAclConnStructDelete(struct l2cAclConn *conn) |
| { |
| if (conn->reassSg) { |
| logw("ACL conn struct delete with %ub still in reassembly buffer\n", sgLength(conn->reassSg)); |
| sgFree(conn->reassSg); |
| } |
| |
| if (conn->next) |
| conn->next->prev = conn->prev; |
| if (conn->prev) |
| conn->prev->next = conn->next; |
| else |
| mAclConns = conn->next; |
| |
| free(conn); |
| } |
| |
| /* |
| * FUNCTION: l2cAclSigNextIdent |
| * USE: Request an "ident" value for signalling channel on ACL link |
| * PARAMS: aclConn - the ACL connection structure |
| * commit - whether to increment internal counter or not |
| * RETURN: the ident value |
| * NOTES: Call with mConnsLock held |
| */ |
| static uint8_t l2cAclSigNextIdent(struct l2cAclConn *aclConn, bool commit) |
| { |
| uint8_t ret = aclConn->ident; |
| |
| if (commit) |
| aclConn->ident = (ret == 0xFF) ? 1 : ret + 1; |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cAclFreeCid |
| * USE: Find a free channel ID for an ACL connection |
| * PARAMS: aclConn - the acl connection to send it on |
| * RETURN: a free channel ID or zero on failure |
| * NOTES: call with mConnsLock held |
| */ |
| static uint16_t l2cAclFreeCid(struct l2cAclConn *aclConn) |
| { |
| struct l2cConn* conn; |
| uint16_t guess = aclConn->nextChan, next, initial = aclConn->nextChan; |
| |
| while(1) { |
| /* calculate next guess */ |
| next = guess + 1; |
| if (next < L2C_NUM_FIXED_CHANNELS) |
| next = L2C_NUM_FIXED_CHANNELS; |
| |
| /* see if the guessed free channel is actually free */ |
| conn = mConns; |
| while (conn && (conn->acl != aclConn->handle || conn->localCh != guess)) |
| conn = conn->next; |
| |
| /* if so, we're done! */ |
| if (!conn) |
| break; |
| |
| /* else move on to the next one */ |
| guess = next; |
| |
| /* unless we've come a full circle, then give up */ |
| if (guess == initial) |
| return 0; |
| } |
| |
| /* store away where the next search should start */ |
| aclConn->nextChan = next; |
| return guess; |
| } |
| |
| /* |
| * FUNCTION: l2cSigSend |
| * USE: Send a generic l2cap signalling frame |
| * PARAMS: aclConn - the ACL connection |
| * ident - ident to use, 0 for auto |
| * code - the ACL command code |
| * sigFrm - the signalling frame to send (a copy will be made) |
| * sigFrmLen - length of said signalling frame |
| * extraPtr - extra data to attach, if any (a copy will be made) |
| * extraLen - length of the extra data |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSend(struct l2cAclConn *aclConn, uint8_t ident, uint8_t code, const void *sigFrm, uint16_t sigFrmLen, const void *extraPtr, uint16_t extraLen) |
| { |
| struct l2cSigFrm sig; |
| bool newIdent = ident == 0; |
| sg pdu; |
| |
| utilSetLE8(&sig.code, code); |
| utilSetLE16(&sig.len, sigFrmLen + extraLen); |
| |
| if (newIdent) |
| ident = l2cAclSigNextIdent(aclConn, false); |
| utilSetLE8(&sig.ident, ident); |
| |
| pdu = sgNew(); |
| if (!pdu) { |
| loge("L2C PDU oom - will not send\n"); |
| return 0; |
| } |
| |
| if (!sgConcatBackCopy(pdu, &sig, sizeof(sig)) || !sgConcatBackCopy(pdu, sigFrm, sigFrmLen) || !sgConcatBackCopy(pdu, extraPtr, extraLen)) { |
| sgFree(pdu); |
| loge("L2C PDU concat error - will not send\n"); |
| return 0; |
| } |
| |
| if (l2cAclEnqueueSigTx(aclConn, pdu)) { |
| if (newIdent) |
| l2cAclSigNextIdent(aclConn, true); |
| return ident; |
| } else { |
| sgFree(pdu); |
| return 0; |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendConnectionReq |
| * USE: Send a request to open a PSM channel |
| * PARAMS: aclConn - the acl connection to send it on |
| * psm - the PSM |
| * localCh - local channel number |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendConnectionReq(struct l2cAclConn *aclConn, psm_t psm, uint16_t localCh) |
| { |
| struct l2cSigConnReq req; |
| |
| utilSetLE16(&req.psm, psm); |
| utilSetLE16(&req.scid, localCh); |
| |
| return l2cSigSend(aclConn, 0, L2C_SIG_CONN_REQ, &req, sizeof(req), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendConnectionRsp |
| * USE: Send a response to an open request for a PSM channel |
| * PARAMS: aclConn - the acl connection to send it on |
| * ident - ident to use |
| * localCh - local channel number |
| * remCh - remote channel number |
| * result - the result value to send |
| * status - the status value to send |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendConnectionRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t remCh, uint16_t result, uint16_t status) |
| { |
| struct l2cSigConnRsp rsp; |
| |
| utilSetLE16(&rsp.dcid, localCh); |
| utilSetLE16(&rsp.scid, remCh); |
| utilSetLE16(&rsp.result, result); |
| utilSetLE16(&rsp.status, status); |
| |
| return l2cSigSend(aclConn, ident, L2C_SIG_CONN_RSP, &rsp, sizeof(rsp), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendConnectionReq |
| * USE: Send a request to configure a PSM channel |
| * PARAMS: aclConn - the acl connection to send it on |
| * remCh - remote channel number |
| * flags - the flags value to send |
| * cfgData - config data to send (copy will be made) |
| * cfgLen - length of config data |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendConfigReq(struct l2cAclConn *aclConn, uint16_t remCh, uint16_t flags, const void *cfgData, uint16_t cfgLen) |
| { |
| struct l2cSigConfReq req; |
| |
| utilSetLE16(&req.dcid, remCh); |
| utilSetLE16(&req.flags, flags); |
| |
| return l2cSigSend(aclConn, 0, L2C_SIG_CONF_REQ, &req, sizeof(req), cfgData, cfgLen); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendConnectionRsp |
| * USE: Send a response to an configure request for a PSM channel |
| * PARAMS: aclConn - the acl connection to send it on |
| * ident - ident to use |
| * remCh - remote channel number |
| * flags - the flags value to send |
| * result - the result value to send |
| * cfgData - config data to send (copy will be made) |
| * cfgLen - length of config data |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendConfigRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t remCh, uint16_t flags, uint16_t result, const void *cfgData, uint16_t cfgLen) |
| { |
| struct l2cSigConfRsp rsp; |
| |
| utilSetLE16(&rsp.scid, remCh); |
| utilSetLE16(&rsp.flags, flags); |
| utilSetLE16(&rsp.result, result); |
| |
| return l2cSigSend(aclConn, ident, L2C_SIG_CONF_RSP, &rsp, sizeof(rsp), cfgData, cfgLen); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendDiscReq |
| * USE: Send a request to close a PSM channel |
| * PARAMS: aclConn - the acl connection |
| * localCh - local channel number |
| * remCh - remote channel number |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendDiscReq(struct l2cAclConn *aclConn, uint16_t localCh, uint16_t remCh) |
| { |
| struct l2cSigDiscReq req; |
| |
| utilSetLE16(&req.scid, localCh); |
| utilSetLE16(&req.dcid, remCh); |
| |
| return l2cSigSend(aclConn, 0, L2C_SIG_DISC_REQ, &req, sizeof(req), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendDiscRsp |
| * USE: Send a response to an close request for a PSM channel |
| * PARAMS: aclConn - the acl connection |
| * ident - ident to use |
| * localCh - local channel number |
| * remCh - remote channel number |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendDiscRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t localCh, uint16_t remCh) |
| { |
| struct l2cSigDiscRsp rsp; |
| |
| utilSetLE16(&rsp.scid, remCh); |
| utilSetLE16(&rsp.dcid, localCh); |
| |
| return l2cSigSend(aclConn, ident, L2C_SIG_DISC_RSP, &rsp, sizeof(rsp), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendEchoRsp |
| * USE: Send a response to an echo request |
| * PARAMS: aclConn - the acl connection to send it on |
| * ident - ident to use |
| * echoData - echo data to send (copy will be made) |
| * echoLen - length of config data |
| * RETURN: ident vlue used for request or 0 if none was sent |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendEchoRsp(struct l2cAclConn *aclConn, uint8_t ident, const void *echoData, uint16_t echoLen) |
| { |
| return l2cSigSend(aclConn, ident, L2C_SIG_ECHO_RSP, NULL, 0, echoData, echoLen); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendInfoReq |
| * USE: Send an info request |
| * PARAMS: aclConn - the acl connection |
| * infoType - requested info type |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendInfoReq(struct l2cAclConn *aclConn, uint16_t infoType) |
| { |
| struct l2cSigInfoReq req; |
| |
| utilSetLE16(&req.infoType, infoType); |
| |
| return l2cSigSend(aclConn, 0, L2C_SIG_INFO_REQ, &req, sizeof(req), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendInfoRsp |
| * USE: Send a response to an info request |
| * PARAMS: aclConn - the acl connection |
| * ident - ident to use |
| * infoType - requested info type |
| * result - requested info type |
| * infoData - info data to send (copy will be made) |
| * infoLen - length of config data |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendInfoRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t infoType, uint16_t result, const void *infoData, uint16_t infoLen) |
| { |
| struct l2cSigInfoRsp rsp; |
| |
| utilSetLE16(&rsp.infoType, infoType); |
| utilSetLE16(&rsp.result, result); |
| |
| return l2cSigSend(aclConn, ident, L2C_SIG_INFO_RSP, &rsp, sizeof(rsp), infoData, infoLen); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendConnUpdateReq |
| * USE: Send a connection update request for an LE link |
| * PARAMS: aclConn - the acl connection |
| * intMin - minimum requested interval in units of 1.25ms |
| * intMax - maximum requested interval in units of 1.25ms |
| * latency - requested latency |
| * timeoutMult - requested timeout in units of 10ms |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendConnUpdateReq(struct l2cAclConn *aclConn, uint16_t intMin, uint16_t intMax, uint16_t latency, uint16_t timeoutMult) |
| { |
| struct l2cSigConnUpdtReq req; |
| |
| utilSetLE16(&req.intMin, intMin); |
| utilSetLE16(&req.intMax, intMax); |
| utilSetLE16(&req.latency, latency); |
| utilSetLE16(&req.timeoutMult, timeoutMult); |
| |
| return l2cSigSend(aclConn, 0, L2C_SIG_CONN_UPDT_REQ, &req, sizeof(req), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendConnUpdateRsp |
| * USE: Send a connection update request for an LE link |
| * PARAMS: aclConn - the acl connection |
| * ident - ident to use |
| * result - requested info type |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendConnUpdateRsp(struct l2cAclConn *aclConn, uint8_t ident, uint16_t result) |
| { |
| struct l2cSigConnUpdtRsp rsp; |
| |
| logd("L2C conn updte rsp: %u with id %u\n", result, ident); |
| utilSetLE16(&rsp.result, result); |
| |
| return l2cSigSend(aclConn, ident, L2C_SIG_CONN_UPDT_RSP, &rsp, sizeof(rsp), NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendErrCmdReject |
| * USE: Send a "command rejected" signalling frame |
| * PARAMS: aclConn - the acl connection |
| * ident - the ident value to use |
| * reason - the reason to send |
| * rejData - rejection data to send (copy will be made) |
| * rejLen - length of rejection data |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendErrCmdReject(struct l2cAclConn *aclConn, uint8_t ident, uint16_t reason, const void *rejData, uint16_t rejLen) |
| { |
| struct l2cSigCmdRej rej; |
| |
| utilSetLE16(&rej.reason, reason); |
| |
| return l2cSigSend(aclConn, ident, L2C_SIG_CMD_REJ, &rej, sizeof(rej), rejData, rejLen); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendErrCmdNotUnderstood |
| * USE: Send a "command not understood" error |
| * PARAMS: aclConn - the acl connection |
| * ident - the ident value to use |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendErrCmdNotUnderstood(struct l2cAclConn *aclConn, uint8_t ident) |
| { |
| return l2cSigSendErrCmdReject(aclConn, ident, L2C_SIG_CMD_NOT_UNDERSTOD, NULL, 0); |
| } |
| |
| /* |
| * FUNCTION: l2cSigSendErrInvalidCid |
| * USE: Send a "invalid channel id" error |
| * PARAMS: aclConn - the acl connection |
| * rxedDcid - the received "dcid" value |
| * rxedScid - the received "scid" value |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cSigSendErrInvalidCid(struct l2cAclConn *aclConn, uint8_t ident, uint16_t rxedDcid, uint16_t rxedScid) |
| { |
| struct l2cSigInvalidCid info; |
| |
| utilSetLE16(&info.rxedDcid, rxedDcid); |
| utilSetLE16(&info.rxedScid, rxedScid); |
| |
| return l2cSigSendErrCmdReject(aclConn, ident, L2C_SIG_INVALID_CID, &info, sizeof(info)); |
| } |
| |
| /* |
| * FUNCTION: l2cConnRequestClose |
| * USE: Request an orderly closing of a channel |
| * PARAMS: acl - the ACL link structure (pass NULL to auto-find it) |
| * conn - the connection struct |
| * tellOtherSide - TX a message to other side? |
| * RETURN: ident if request was sent, else 0 |
| * NOTES: Call with mConnsLock held. Will call service's free(), cbk if appropriate |
| */ |
| static void l2cConnRequestClose(struct l2cAclConn* acl, struct l2cConn *conn, bool tellOtherSide) |
| { |
| struct l2cConn *t = mConns; |
| uint32_t numConnsOnThisAcl = 0; |
| bool destroyStruct = false; |
| bool tellOurSide = true; |
| bool closeLink = false; |
| |
| /* find the ACL connection, if none was given */ |
| if (!acl) |
| acl = l2cAclConnFindByHandle(conn->acl); |
| |
| if (!acl) { |
| loge("Failed to close connection psm %d chs %d %d because acl link was not found\n", conn->psm, conn->localCh, conn->remCh); |
| return; |
| } |
| |
| /* see how many connections exist on this ACL link */ |
| while (t) { |
| if (t->acl == acl->handle && t->inUse) |
| numConnsOnThisAcl++; |
| t = t->next; |
| } |
| |
| if (!numConnsOnThisAcl) { |
| loge("Number of in-use connections on acl "HCI_CONN_FMT"x: %u\n", HCI_CONN_CONV(acl->conn), numConnsOnThisAcl); |
| numConnsOnThisAcl = 1; |
| } |
| |
| switch (conn->state) { |
| case L2C_CONN_STATE_CONN_WAIT: |
| if (numConnsOnThisAcl == 1) |
| destroyStruct = true; |
| tellOtherSide = false; |
| closeLink = true; |
| break; |
| case L2C_CONN_STATE_RX_OPEN_SVC: |
| /* cannot destroy struct now, mark it for deletion when service alloc is done */ |
| conn->state = L2C_CONN_STATE_TEARDOWN; |
| tellOtherSide = tellOurSide = false; /* we do not need to do this now */ |
| break; |
| case L2C_CONN_STATE_TX_OPEN_TXED: |
| /* |
| * The spec has no way for us to cancel a connection request, and we don't want |
| * to bother waiting for an unbound time for establishment. We'll just drop it |
| * and let the other side deal with it. XXX: should we be wiser about this? |
| */ |
| destroyStruct = true; |
| break; |
| |
| case L2C_CONN_STATE_ENCR_WAIT: |
| tellOurSide = false; /* nodody to tell to */ |
| case L2C_CONN_STATE_ATHORIZ_WAIT: |
| case L2C_CONN_STATE_CFG: |
| case L2C_CONN_STATE_ESTABLISHED: |
| destroyStruct = true; |
| break; |
| |
| case L2C_CONN_STATE_DIE_REQD: |
| case L2C_CONN_STATE_DIE_RXED: |
| case L2C_CONN_STATE_TEARDOWN: |
| /* nothing to do here - we're already on it*/ |
| tellOtherSide = tellOurSide = false; /* we do not need to do this now */ |
| break; |
| |
| default: |
| loge("Unknown state %d in disc. this is invariably bad.", conn->state); |
| destroyStruct = true; |
| break; |
| } |
| if (tellOurSide) |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to tell our side that connection is closed\n"); |
| |
| tellOtherSide = tellOtherSide && conn->psm; |
| if (tellOtherSide) |
| if (!l2cSigSendDiscReq(acl, conn->localCh, conn->remCh)) |
| loge("Failed to send disc req msg to the other side\n"); |
| if (destroyStruct) |
| l2cConnStructDelete(conn); |
| /* |
| * If you ever have both tellOtherSide and closeLink set, you'll have a bad time |
| * since data may not go out befor elink dies. we do not do that, but if you ever |
| * want to, heed this warning and add acl link destruction to the work queue |
| * instead of doing it here. |
| */ |
| if (closeLink) |
| l2cAclDownTimerStart(acl); |
| } |
| |
| /* |
| * FUNCTION: l2cApiServicePsmRegister |
| * USE: Registers a service that works on top of L2CAP PSM-based channels |
| * PARAMS: psm - the PSM you want, or zero to be assigned one |
| * service - the service descriptor for the service |
| * RETURN: the psm you were assigned or zero in case of some error |
| * NOTES: service callbacks may be called before this function returns |
| * DOES NOT AND CANNOT TAKE mConnsLock |
| */ |
| psm_t l2cApiServicePsmRegister(psm_t psm, const struct l2cServicePsmDescriptor *service) |
| { |
| struct l2cPsmSvc *svc; |
| |
| if (!l2cPsmValid(psm)) { |
| logw("Invalid PSM %d\n", psm); |
| return false; |
| } |
| |
| if (!service->mtu && service->serviceInstanceAlloc) { |
| logw("Invalid MTU 0 for conn-oriented svc for psm %d\n", psm); |
| return false; |
| } |
| |
| svc = (struct l2cPsmSvc*)calloc(1, sizeof(struct l2cPsmSvc)); |
| if (!svc) |
| return false; |
| |
| memcpy(&svc->desc, service, sizeof(svc->desc)); |
| svc->beingRemoved = false; |
| svc->prev = NULL; |
| |
| pthread_mutex_lock(&mSvcsLock); |
| if (psm) { /* check for existence */ |
| |
| if (l2cPsmSvcFindByPsm(psm)) |
| psm = 0; |
| } else { /* find a free one */ |
| psm_t orig_next_psm = mNextPsm; |
| |
| do { |
| uint64_t mNextPsm_t = mNextPsm + 2; /* only odds allowed */ |
| psm = mNextPsm; |
| |
| /* calculate the next potential valid PSM */ |
| mNextPsm += 2; |
| if (mNextPsm_t & 0x0100) /* no low bit on high byte alowed */ |
| mNextPsm_t += 0x100; |
| if (mNextPsm_t < 0x1000 || mNextPsm_t > 0xFFFF) /* enforce max and min */ |
| mNextPsm_t = 0x1001; |
| mNextPsm = mNextPsm_t; |
| |
| /* check if existing PSM is taken */ |
| if (l2cPsmSvcFindByPsm(psm)) |
| psm = 0; |
| else |
| break; |
| |
| } while(mNextPsm != orig_next_psm); |
| } |
| |
| if (psm) { /* we have one that we can use */ |
| |
| svc->psm = psm; |
| svc->next = mPsmSvcs; |
| if (mPsmSvcs) |
| mPsmSvcs->prev = svc; |
| mPsmSvcs = svc; |
| } else |
| free(svc); |
| |
| pthread_mutex_unlock(&mSvcsLock); |
| |
| return psm; |
| } |
| |
| /* |
| * FUNCTION: l2cApiServicePsmUnregister |
| * USE: Unregisters a service that works on top of L2CAP PSM-based channels |
| * PARAMS: psm - the PSM that was previously registered |
| * doneCbk - called when removal is finished (can be NULL) |
| * callbackData - data to pass to callback |
| * RETURN: true success |
| * NOTES: Your job to make sure service is not in use. |
| */ |
| bool l2cApiServicePsmUnregister(psm_t psm, void (*doneCbk)(void*), void* cbkData) |
| { |
| bool success = false; |
| struct l2cPsmSvc *svc; |
| struct l2cConn *conn; |
| bool anythingDone; |
| |
| /* terminate all connections with that psm that the other side opened */ |
| pthread_mutex_lock(&mConnsLock); |
| do { |
| anythingDone = false; |
| conn = mConns; |
| while (conn){ |
| if (conn->psm == psm && !conn->weOpened) { |
| |
| l2cConnRequestClose(NULL, conn, true); |
| anythingDone = true; |
| break; |
| } |
| conn = conn->next; |
| } |
| |
| } while(anythingDone); |
| pthread_mutex_unlock(&mConnsLock); |
| |
| /* schedule demolition of the psm itself */ |
| /* cannot take services lock staring here */ |
| pthread_mutex_lock(&mSvcsLock); |
| svc = l2cPsmSvcFindByPsm(psm); |
| if (svc) { |
| svc->beingRemoved = true; |
| l2cSvcWorkSchedulePsmUnregister(psm, doneCbk, cbkData); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| |
| return success; |
| } |
| |
| /* |
| * FUNCTION: l2cApiServiceFixedChRegister |
| * USE: Registers a service that works on top of L2CAP FixedCh |
| * PARAMS: chan - the channel you want |
| * service - the service descriptor for the service |
| * RETURN: truwe on success |
| * NOTES: service callbacks may be called before this function returns |
| * DOES NOT AND CANNOT TAKE mConnsLock |
| */ |
| bool l2cApiServiceFixedChRegister(uint16_t chan, const struct l2cServiceFixedChDescriptor *service) |
| { |
| struct l2cFixedChSvc *svc; |
| |
| if (chan == L2C_CH_SIGNALLING_EDR || chan == L2C_CH_CONNECTIONLESS || |
| chan == L2C_CH_SIGNALLING_LE || !chan || chan >= L2C_NUM_FIXED_CHANNELS) |
| return false; |
| |
| svc = (struct l2cFixedChSvc*)calloc(1, sizeof(struct l2cFixedChSvc)); |
| if (!svc) |
| return false; |
| |
| memcpy(&svc->desc, service, sizeof(svc->desc)); |
| svc->beingRemoved = false; |
| svc->prev = NULL; |
| |
| pthread_mutex_lock(&mSvcsLock); |
| if (chan && !l2cFixedChSvcFindByChan(chan)) { /* check for validity & existence */ |
| |
| svc->chan = chan; |
| svc->next = mFixedSvcs; |
| if (mFixedSvcs) |
| mFixedSvcs->prev = svc; |
| mFixedSvcs = svc; |
| } else { |
| chan = 0; |
| free(svc); |
| } |
| |
| pthread_mutex_unlock(&mSvcsLock); |
| |
| return chan != 0; |
| } |
| |
| /* |
| * FUNCTION: l2cApiServiceFixedChUnregister |
| * USE: Unregisters a service that works on top of L2CAP FixedCh |
| * PARAMS: chan - the channel |
| * doneCbk - called when removal is finished (can be NULL) |
| * callbackData - data to pass to callback |
| * RETURN: true success |
| * NOTES: Your job to make sure service is not in use. |
| */ |
| bool l2cApiServiceFixedChUnregister(uint16_t chan, void (*doneCbk)(void*), void* cbkData) |
| { |
| bool success = false; |
| struct l2cFixedChSvc *svc; |
| struct l2cConn *conn; |
| bool anythingDone; |
| |
| /* terminate all connections to that FixedCh that the other side opened */ |
| pthread_mutex_lock(&mConnsLock); |
| do { |
| anythingDone = false; |
| conn = mConns; |
| while(conn) { |
| if (!conn->psm && conn->localCh == chan && !conn->weOpened && conn->state != L2C_CONN_STATE_TEARDOWN) { |
| |
| if (conn->instance) { |
| conn->state = L2C_CONN_STATE_TEARDOWN; |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to send close state\n"); |
| } else { |
| logw("no instance found for fixed ch %d while unregistering\n", chan); |
| l2cConnStructDelete(conn); |
| } |
| anythingDone = true; |
| break; |
| } |
| conn = conn->next; |
| } |
| |
| } while(anythingDone); |
| pthread_mutex_unlock(&mConnsLock); |
| |
| /* schedule demolition of the fixedCh registration itself */ |
| /* cannot take services lock staring here */ |
| pthread_mutex_lock(&mSvcsLock); |
| svc = l2cFixedChSvcFindByChan(chan); |
| if (svc) { |
| svc->beingRemoved = true; |
| l2cSvcWorkScheduleFixedChUnregister(chan, doneCbk, cbkData); |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| |
| return success; |
| } |
| |
| /* |
| * FUNCTION: l2cConnFindByHandle |
| * USE: Find a connection structure by handle |
| * PARAMS: handle - the handle |
| * RETURN: connection structure or NULL |
| * NOTES: Call with mConnsLock held. Not possible for connections |
| * in conn-wait state since they do not have valid handles |
| * due to lack of an acl channel if |
| */ |
| static struct l2cConn* l2cConnFindByHandle(l2c_handle_t handle) |
| { |
| struct l2cConn *conn; |
| |
| conn = mConns; |
| |
| while (conn && conn->handle != handle) |
| conn = conn->next; |
| |
| return conn; |
| } |
| |
| /* |
| * FUNCTION: l2cApiGetBtAddr |
| * USE: Get BT addr of whoever is on the other end of a pipe |
| * PARAMS: handle - the handle |
| * addrP - here is stored the address |
| * RETURN: true on success |
| * NOTES: |
| */ |
| bool l2cApiGetBtAddr(l2c_handle_t handle, struct bt_addr* addrP) |
| { |
| struct l2cAclConn *aclConn; |
| struct l2cConn* conn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cConnFindByHandle(handle); |
| if (conn) { |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (aclConn) { |
| memcpy(addrP, &aclConn->peerAddr, sizeof(struct bt_addr)); |
| ret = true; |
| } |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiGetSelfBtAddr |
| * USE: Get BT addr we presented when we made the connection |
| * PARAMS: handle - the handle |
| * addrP - here is stored the address |
| * RETURN: true on success |
| * NOTES: |
| */ |
| bool l2cApiGetSelfBtAddr(l2c_handle_t handle, struct bt_addr *addrP) |
| { |
| struct l2cAclConn *aclConn; |
| struct l2cConn* conn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cConnFindByHandle(handle); |
| if (conn) { |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (aclConn) { |
| memcpy(addrP, &aclConn->selfAddr, sizeof(struct bt_addr)); |
| ret = true; |
| } |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiGetAclHandle |
| * USE: Get ACL handle for the L2CAP connection |
| * PARAMS: handle - the handle |
| * RETURN: ACL handle, or zero for invalid L2CAP handle |
| * NOTES: |
| */ |
| acl_handle_t l2cApiGetAclHandle(l2c_handle_t handle) |
| { |
| struct l2cConn* conn; |
| acl_handle_t ret = 0; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cConnFindByHandle(handle); |
| if (conn) { |
| ret = conn->acl; |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return ret; |
| } |
| |
| |
| /* |
| * FUNCTION: l2cApiDisconnect |
| * USE: Request a disconnect of a pipe |
| * PARAMS: handle - the handle |
| * RETURN: NONE |
| * NOTES: Disconnect is async and will finish later |
| */ |
| void l2cApiDisconnect(l2c_handle_t handle) |
| { |
| struct l2cConn *conn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (conn) |
| l2cConnRequestClose(NULL, conn, true); |
| else |
| logw("Got request to close nonexistent handle "HANDLEFMT"\n", HANDLECNV(handle)); |
| |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cTryMakeSendProgress |
| * USE: Try to make forward progress on sending data to HCI for radio transmssion. |
| * Called when we either have new data or new credits. |
| * PARAMS: forLeBuffers - which set of buffers we should work on |
| * RETURN: NONE |
| * NOTES: will take mConnsLock |
| */ |
| static void l2cTryMakeSendProgress(bool forLeBuffers) |
| { |
| struct l2cSigPduSendItem *picked = NULL; |
| struct l2cSigPduSendItem **sigQHP; |
| struct l2cSigPduSendItem **sigQTP; |
| uint16_t chipBufSz; |
| hci_conn_t *qConnP; |
| bool *qFirstP; |
| uint8_t ret; |
| sg *qBufP; |
| hci_conn_t sendConn; |
| bool sendFirst; |
| sg sendSg, remainSg; |
| bool hadData = true; |
| |
| while(1) { |
| pthread_mutex_lock(&mConnsLock); |
| |
| if (forLeBuffers) { |
| qBufP = &mQueueBufLe; |
| qFirstP = &mQueueFirstLe; |
| qConnP = &mQueueConnLe; |
| sigQHP = &sigSendHeadLe; |
| sigQTP = &sigSendTailLe; |
| chipBufSz = mChipBufLenLe; |
| } else { |
| qBufP = &mQueueBufEdr; |
| qFirstP = &mQueueFirstEdr; |
| qConnP = &mQueueConnEdr; |
| sigQHP = &sigSendHeadEdr; |
| sigQTP = &sigSendTailEdr; |
| chipBufSz = mChipBufLenEdr; |
| } |
| |
| /* |
| * We always send SIG frames ahead of DATA frames, but we cannot do that if we |
| * already sent a fragmented data frame's start on the same ACL link, so we |
| * check for that special case here. We can reorder SIG frames for different |
| * links though. So this case only bites us if we only have SIG frames enqueued |
| * for the same link we have data frame half-sent on. The code segment that |
| * follows will detect if we cannot send SIG frames for some conection at this |
| * time and will find us another SIG frame to send, if there is one. Else it will |
| * just pick the first SIG frame. A further complication is if we pick a SIG frame |
| * that does not fit in the controller's buffer. In this case we'd have to somehow |
| * rememebr to send the rest of it later, but if our queue already has a DATA |
| * packet it in (in a half-sent state), we're royally boned. For this case, we also |
| * avoid SIG packets over controller's buffer size while we have a half-sent DATA |
| * packet in the queue. |
| */ |
| picked = *sigQHP; |
| while (picked && *qBufP && ((!*qFirstP && picked->aclConnID == *qConnP) || (sgLength(picked->pdu) > chipBufSz)) ) |
| picked = picked->next; |
| |
| if (picked){ /* we have a SIG frame to send */ |
| sendConn = picked->aclConnID; |
| sendSg = picked->pdu; |
| sendFirst = true; |
| |
| if (!picked->hasL2cHdr) { |
| struct l2cFrmHdr frm; |
| |
| utilSetLE16(&frm.len, sgLength(picked->pdu)); |
| utilSetLE16(&frm.cid, picked->cid); |
| |
| if (!sgConcatFrontCopy(picked->pdu, &frm, sizeof(frm))) |
| loge("catastrophic failure in concatting l2c header to SIG frm\n"); |
| else |
| picked->hasL2cHdr = true; |
| } |
| |
| } else { /* we continue the existing DATA frame */ |
| sendConn = *qConnP; |
| sendSg = *qBufP; |
| sendFirst = *qFirstP; |
| } |
| |
| if (!sendConn) { /* nothing to send */ |
| pthread_mutex_unlock(&mConnsLock); |
| if (!hadData) |
| break; |
| hadData = false; |
| if (forLeBuffers || !l2cAdvMayTx()) |
| multiNotifNotify(mWriteNotif, NULL); |
| continue; |
| } |
| hadData = true; |
| |
| if (sgLength(sendSg) > chipBufSz) { |
| remainSg = sgSplit(sendSg, chipBufSz); |
| if (!remainSg) { |
| loge("Failed to split TX sg\n"); |
| ///TODO: this? |
| } |
| } else |
| remainSg = NULL; |
| |
| ret = hciTryToTx(sendConn, sendSg, sendFirst ? (remainSg ? HCI_BOUNDARY_START : HCI_BOUNDARY_COMPLETE) : HCI_BOUNDARY_CONT); |
| if (ret == HCI_TX_SEND_ERROR) |
| logw("State indeterminate after hci tx fail!\n"); |
| |
| if (ret == HCI_TX_SEND_OK || ret == HCI_TX_SEND_NO_CONN || ret == HCI_TX_SEND_ERROR) { /* either way we do not resend this in its current state */ |
| |
| if (ret != HCI_TX_SEND_OK) |
| sgFree(sendSg); |
| |
| if (picked) { /* dequeue the SIG frame we [partially] sent */ |
| if (picked->prev) |
| picked->prev->next = picked->next; |
| else |
| *sigQHP = picked->next; |
| |
| if (picked->next) |
| picked->next->prev = picked->prev; |
| else |
| *sigQTP = picked->prev; |
| |
| free(picked); |
| } else { /* clear out the global queue state - if needed it will be reset later */ |
| *qConnP = 0; |
| *qBufP = NULL; |
| *qFirstP = true; |
| } |
| } |
| |
| if (ret == HCI_TX_SEND_ERROR && remainSg) { |
| logw("Dropping continuation of a msg that failed to send\n"); |
| sgFree(remainSg); |
| remainSg = NULL; |
| } |
| |
| if (ret == HCI_TX_SEND_OK || ret == HCI_TX_SEND_ERROR) { |
| |
| if (remainSg) { /* not done? enqueue the rest of the fragment */ |
| *qConnP = sendConn; |
| *qBufP = remainSg; |
| *qFirstP = false; |
| } |
| } |
| |
| /* releasing the lock here allows threads waiting to write to do so and helps keep the pipeline full */ |
| pthread_mutex_unlock(&mConnsLock); |
| |
| if (ret == HCI_TX_SEND_NO_CREDITS) |
| break; |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cApiRegisterWriteBufNotif |
| * USE: Register for write buffer notifs |
| * PARAMS: cbk - the cbk to call |
| * cbkData - the data to pass to it |
| * RETURN: handle fo the notif for l2cApiUnegisterWriteBufNotif() |
| * NOTES: cbk may be called before this func returns |
| */ |
| uniq_t l2cApiRegisterWriteBufNotif(multiNotifCbk cbk, void *cbkData) |
| { |
| return multiNotifRegister(mWriteNotif, cbk, cbkData); |
| } |
| |
| /* |
| * FUNCTION: l2cApiUnegisterWriteBufNotif |
| * USE: Unregister from write buffer notif created by l2cApiRegisterWriteBufNotif() |
| * PARAMS: notifHandle - the notif handle |
| * RETURN: NONE |
| * NOTES: cbk will definitely not be called once this func returns |
| */ |
| void l2cApiUnegisterWriteBufNotif(uniq_t notifHandle) |
| { |
| multiNotifUnregister(mWriteNotif, notifHandle); |
| } |
| |
| /* |
| * FUNCTION: l2cAclCreditAvail |
| * USE: Called by lower layers when a send credit becomes available |
| * PARAMS: le - was it an LE credit |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cAclCreditAvail(bool le) |
| { |
| if (!l2cSvcWorkScheduleTryTx(le)) |
| loge("Failed to schedule TX attempt\n"); |
| } |
| |
| /* |
| * FUNCTION: l2cDataSendOnAclLink |
| * USE: Request a send on an ACL link of data with a header |
| * PARAMS: aclConn - the link |
| * hdr - header to prepend IFF accepted for TX (can be NULL if hdrLen is 0) |
| * hdrLen - size fo said header |
| * data - the data to send (can be null if hdrLen is nonzero) |
| * RETURN: L2C_TX_* |
| * NOTES: call with mConnsLock held |
| */ |
| static uint8_t l2cDataSendOnAclLink(struct l2cAclConn *aclConn, const void *hdr, uint32_t hdrLen, sg data) |
| { |
| uint8_t ret = L2C_TX_ACCEPTED; |
| sg *queueBufP; |
| hci_conn_t *queueConnP; |
| bool *queueFirstP; |
| bool useLeBuffers; |
| bool freeData = false; |
| |
| /* see which queue (LE/EDR) we need to use */ |
| useLeBuffers = BT_ADDR_IS_LE(aclConn->peerAddr) && !mChipJointBuffers; |
| if (useLeBuffers) { |
| queueBufP = &mQueueBufLe; |
| queueFirstP = &mQueueFirstLe; |
| queueConnP = &mQueueConnLe; |
| } else { |
| queueBufP = &mQueueBufEdr; |
| queueFirstP = &mQueueFirstEdr; |
| queueConnP = &mQueueConnEdr; |
| } |
| |
| /* if queue is not busy, make it busy */ |
| if (*queueBufP) |
| ret = L2C_TX_TRY_LATER; |
| else { |
| if (!data){ |
| data = sgNew(); |
| freeData = true; |
| } |
| |
| if (!data) { |
| loge("Failed to alloc sg for data-free l2c acl send\n"); |
| ret = L2C_TX_ERROR; |
| } else if (!sgConcatFrontCopy(data, hdr, hdrLen)) { |
| loge("Failed to prepend L2C header\n"); |
| ret = L2C_TX_ERROR; |
| } else { |
| *queueBufP = data; |
| *queueFirstP = true; |
| *queueConnP = aclConn->conn; |
| if (!l2cSvcWorkScheduleTryTx(useLeBuffers)) |
| loge("Failed to schedule TX attempt\n"); |
| } |
| } |
| |
| if (ret != L2C_TX_ACCEPTED && freeData && data) |
| sgFree(data); |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiDataTx |
| * USE: Request a send on a pipe |
| * PARAMS: handle - the handle |
| * data - the data |
| * RETURN: L2C_TX_* |
| * NOTES: Never blocks! |
| */ |
| uint8_t l2cApiDataTx(l2c_handle_t handle, sg data) |
| { |
| struct l2cAclConn *aclConn = NULL; |
| uint8_t ret = L2C_TX_ACCEPTED; |
| struct l2cConn *conn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cConnFindByHandle(handle); |
| if (conn) |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| |
| /* we only support L2CAP simple mode for now */ |
| |
| if (!conn) { |
| ret = L2C_TX_NO_CONN; |
| logw("Request send on invalid handle "HANDLEFMT"\n", HANDLECNV(handle)); |
| } else if (!aclConn) { |
| ret = L2C_TX_NO_CONN; |
| logw("Request send on invalid ACL handle "HANDLEFMT"\n", HANDLECNV(conn->acl)); |
| } else if (conn->state != L2C_CONN_STATE_ESTABLISHED) { |
| /* |
| * In this case, we could send a retry-later return code, but if you tried to send |
| * at this point, you clearly do not know right from wrong, and do not deserve |
| * another chance |
| */ |
| ret = L2C_TX_ERROR; |
| logw("Attempt to send data over connection in state %d\n", conn->state); |
| } else if (sgLength(data) > conn->theirMtu) { |
| ret = L2C_TX_ERROR; |
| logw("Attempt to send data over MTU size (%u > %u)\n", sgLength(data), conn->theirMtu); |
| } else { |
| struct l2cFrmHdr frm; |
| |
| if (!conn->inUse) { |
| logd("Marking conn "HANDLEFMT" as in use due to TX\n", HANDLECNV(conn->handle)); |
| conn->inUse = 1; |
| l2cAclDownTimerStop(aclConn); |
| } |
| |
| if (conn->mode == L2C_MODE_BASIC) { |
| |
| utilSetLE16(&frm.len, sgLength(data)); |
| utilSetLE16(&frm.cid, conn->remCh); |
| |
| ret = l2cDataSendOnAclLink(aclConn, &frm, sizeof(frm), data); |
| } else |
| ret = l2cAdvUserTx(conn, data); |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiConnectionlessDataTx |
| * USE: Request a send on a connectionless pipe |
| * PARAMS: addr - the peer address |
| * psm - the PSM to send to |
| * data - the data |
| * RETURN: L2C_TX_* |
| * NOTES: Never blocks! |
| */ |
| uint8_t l2cApiConnectionlessDataTx(const struct bt_addr *addr, psm_t psm, sg data) |
| { |
| struct l2cAclConn* aclConn; |
| bool ret; |
| |
| if (!l2cPsmValid(psm)) { |
| logw("Invalid PSM %d\n", psm); |
| return false; |
| } |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindByAddr(addr); |
| if (!aclConn) { |
| logw("Dropping connectionless send to a device whom we aren't connected to\n"); |
| ret = L2C_TX_ERROR; |
| } else if (!BT_ADDR_IS_EDR(aclConn->peerAddr)) { |
| logw("Dropping connectionless send to an LE device\n"); |
| ret = L2C_TX_ERROR; |
| } else if (aclConn->connectionlessMtu < sgLength(data)) { |
| logw("Dropping connectionless send over MTU %u > %u\n", sgLength(data), aclConn->connectionlessMtu); |
| ret = L2C_TX_ERROR; |
| } else { |
| uint8_t hdr[sizeof(struct l2cFrmHdr) + sizeof(psm_t)]; |
| struct l2cFrmHdr *frm = (struct l2cFrmHdr*)hdr; |
| |
| utilSetLE16(&frm->len, sgLength(data) + sizeof(psm_t)); |
| utilSetLE16(&frm->cid, L2C_CH_CONNECTIONLESS); |
| utilSetLE16(frm + 1, psm); |
| |
| ret = l2cDataSendOnAclLink(aclConn, hdr, sizeof(hdr), data); |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiLeEncryptConn |
| * USE: Encrypt a connection with the given key |
| * PARAMS: handle - L2CAP connection handle |
| * rand - random value |
| * ediv - encrypted diversifier value |
| * key - LTK or STK applied during encryption |
| * RETURN: true if encryption is requested; false otherwise |
| * NOTES: |
| */ |
| bool l2cApiLeEncryptConn(l2c_handle_t handle, uint64_t rand, uint16_t ediv, const uint8_t *key) |
| { |
| struct l2cAclConn *aclConn; |
| hci_conn_t aclConnId; |
| struct l2cConn *conn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Cannot find Connection to encrypt\n"); |
| } else { |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) |
| loge("Cannot find Connection to encrypt\n"); |
| else if (aclConn->encrypted) |
| logw("Connection was encrypted already\n"); |
| else { |
| aclConnId = aclConn->conn; |
| pthread_mutex_unlock(&mConnsLock); |
| return hciLeEncryptConn(aclConnId, rand, ediv, key); |
| } |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| |
| return false; |
| } |
| |
| /* |
| * FUNCTION: l2ccAclLinkUpTimeout |
| * USE: Timeout timer for ACL link opening |
| * PARAMS: which - the timer ID |
| * aclConnHandle - acl connection handle |
| * RETURN: tNONE |
| * NOTES: |
| */ |
| static void l2cAclLinkUpTimeout(uniq_t which, uint64_t aclConnHandle) |
| { |
| struct l2cAclConn *aclConn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindByHandle(aclConnHandle); |
| if (aclConn && aclConn->openTimer == which) { |
| logi("ACL link open failed for "HANDLEFMT"\n", HANDLECNV(aclConn->handle)); |
| aclConn->state = L2C_ACL_STATE_TEARDOWN; |
| if (!l2cSvcWorkScheduleAclLinkClose(aclConn->handle)) |
| loge("Failed to schedule call to close ACL link "HANDLEFMT"\n", HANDLECNV(aclConn->handle)); |
| l2cAclLinkDownInt(aclConn, false); |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cCreateAndOpenNewAclConn |
| * USE: Request an ACL connection be created |
| * PARAMS: addr - the peer address |
| * RETURN: true is it was requested. False on immediate error. |
| * NOTES: call with mConnsLock held |
| */ |
| static struct l2cAclConn* l2cCreateAndOpenNewAclConn(const struct bt_addr* addr) |
| { |
| struct l2cAclConn *conn; |
| |
| conn = l2cAclConnFindByAddr(addr); |
| if (conn) { |
| if (conn->state == L2C_ACL_STATE_TEARDOWN) { |
| logw("Asked to establish existing ACL conn as it goes down. Bad luck\n"); |
| return NULL; |
| } else { |
| logw("Asked to establish existing ACL conn\n"); |
| return conn; |
| } |
| } |
| |
| conn = (struct l2cAclConn*)calloc(1, sizeof(struct l2cAclConn)); |
| if (!conn) { |
| loge("Out of memory\n"); |
| return NULL; |
| } |
| |
| memcpy(&conn->peerAddr, addr, sizeof(conn->peerAddr)); |
| conn->handle = uniqGetNext(); |
| conn->state = L2C_ACL_STATE_PENDING; |
| conn->prev = NULL; |
| conn->reassSg = NULL; |
| conn->sigMtu = L2C_MIN_SIG_MTU; |
| |
| if (!l2cSvcWorkScheduleAclLinkOpen(&conn->peerAddr)) { |
| free(conn); |
| conn = NULL; |
| } else { |
| conn->next = mAclConns; |
| if (mAclConns) |
| mAclConns->prev = conn; |
| mAclConns = conn; |
| conn->openTimer = timerSet(L2C_ACL_LINK_UP_TIMEOUT, l2cAclLinkUpTimeout, conn->handle); |
| if (!conn->openTimer) |
| loge("Failed to set open timeout for ACL link\n"); |
| } |
| |
| return conn; |
| } |
| |
| /* |
| * FUNCTION: l2cApiCreatePsmConnection |
| * USE: Request a PSM-based connection be created |
| * PARAMS: psm - the PSM to connect to |
| * addr - the peer address |
| * mtu - the mtu we're willing to accept |
| * doneCbk - function to call when action completes |
| * userData - data to pass to stateCbk |
| * instance - data to pass to stateCbk |
| * RETURN: true is a send was requested. False on immediate error. |
| * NOTES: callback may be called before this function returns, if |
| * true was returned, at least one call to the callback |
| * will happen |
| */ |
| bool l2cApiCreatePsmConnection(psm_t psm, const struct bt_addr* addr, uint16_t mtu, l2cStateCbk stateCbk, void* userData, void* instance) |
| { |
| struct l2cAclConn *aclConn; |
| struct l2cConn *conn = NULL; |
| bool ret = false; |
| |
| if (!stateCbk) { |
| logw("Invalid stateCbk\n"); |
| return false; |
| } |
| |
| if (!l2cPsmValid(psm)) { |
| logw("Invalid PSM %d\n", psm); |
| return false; |
| } |
| |
| if (BT_ADDR_IS_LE(*addr)) { |
| logw("Cannot create a PSM connection to an LE address\n"); |
| return false; |
| } |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindByAddr(addr); |
| if (aclConn && aclConn->state == L2C_ACL_STATE_TEARDOWN) { |
| logw("Trying to open a connection over a going-down ACL link. Bad luck\n"); |
| goto out; |
| } else if (!aclConn) { |
| logd("acl not found for connection requested, making one\n"); |
| aclConn = l2cCreateAndOpenNewAclConn(addr); |
| } |
| if (!aclConn) { |
| loge("Failed to create/open/find ACL conn\n"); |
| goto out; |
| } |
| |
| conn = l2cNewConnStruct(L2C_CONN_STATE_CONN_WAIT, aclConn->handle, 0, 0, psm, mtu, true); |
| if (!conn) { |
| loge("Failed to allocate an L2C conn\n"); |
| goto out; |
| } |
| |
| /* create the conn */ |
| conn->stateCbk = stateCbk; |
| conn->userData = userData; |
| conn->instance = instance; |
| conn->inUse = 1; |
| |
| if (aclConn->conn == L2C_ACL_STATE_ESTABLISHED) { |
| uint8_t ident; |
| uint16_t localCh = l2cAclFreeCid(aclConn); |
| |
| if (!localCh) { |
| logw("No free channels for acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| goto out; |
| } |
| |
| ident = l2cSigSendConnectionReq(aclConn, psm, localCh); |
| if (!ident) { |
| l2cSvcWorkScheduleStateCallDirect(stateCbk, userData, instance, L2C_STATE_CLOSED, NULL, 0); |
| loge("Failed to request opening of a psm channel\n"); |
| goto out; |
| } |
| conn->localCh = localCh; |
| conn->state = L2C_CONN_STATE_TX_OPEN_TXED; |
| conn->ident = ident; |
| l2cAclDownTimerStop(aclConn); |
| l2cConnTimer(conn, L2C_L2CAP_OPEN_TIMEOUT); |
| } |
| conn->next = mConns; |
| if (mConns) |
| mConns->prev = conn; |
| mConns = conn; |
| ret = true; |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| if (!ret && conn) |
| free(conn); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiCreateFixedChConnection |
| * USE: Request a FixedCh connection be created |
| * PARAMS: chan - the channel to connect to |
| * addr - the peer address |
| * stateCbk - function to call with state |
| * userData - data to pass to stateCbk |
| * instance - data to pass to stateCbk |
| * RETURN: true is a send was requested. False on immediate error. |
| * NOTES: callback may be called before this function returns, if |
| * true was returned, at least one call to the callback |
| * will happen |
| */ |
| bool l2cApiCreateFixedChConnection(uint16_t chan, const struct bt_addr* addr, l2cStateCbk stateCbk, void* userData, void* instance) |
| { |
| struct l2cAclConn *aclConn; |
| struct l2cConn *conn = NULL; |
| bool ret = false; |
| |
| if (chan == L2C_CH_SIGNALLING_EDR || chan == L2C_CH_CONNECTIONLESS || |
| chan == L2C_CH_SIGNALLING_LE || !chan || chan >= L2C_NUM_FIXED_CHANNELS) { |
| logw("Invalid channel for creating a FixedCh connection\n"); |
| return false; |
| } |
| |
| if (!stateCbk) { |
| logw("Invalid stateCbk\n"); |
| return false; |
| } |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindByAddr(addr); |
| if (aclConn && aclConn->state == L2C_ACL_STATE_TEARDOWN) { |
| logw("Trying to open a connection over a going-down ACL link. Bad luck\n"); |
| goto out; |
| } else if (!aclConn) { |
| logd("acl not found for connection requested, making one\n"); |
| aclConn = l2cCreateAndOpenNewAclConn(addr); |
| } |
| if (!aclConn) { |
| loge("Failed to create/open/find ACL conn\n"); |
| goto out; |
| } |
| |
| /* check is same FixedCh already exists */ |
| conn = mConns; |
| while(conn && (conn->acl != aclConn->handle || conn->psm || conn->localCh != chan)) |
| conn = conn->next; |
| if (conn) { |
| logi("Ignoring attempt to open FixedCh %d conn when existing exists\n", chan); |
| goto out; |
| } |
| |
| conn = l2cNewConnStruct(L2C_CONN_STATE_CONN_WAIT, aclConn->handle, chan, chan, 0, 0, true); |
| if (!conn) { |
| loge("Failed to allocate an L2C conn\n"); |
| return false; |
| } |
| |
| /* create the conn */ |
| conn->handle = uniqGetNext(); |
| conn->stateCbk = stateCbk; |
| conn->userData = userData; |
| conn->instance = instance; |
| conn->inUse = 1; |
| |
| if (aclConn->conn == L2C_ACL_STATE_ESTABLISHED) { |
| l2cChannelReadyForData(aclConn, conn); |
| l2cAclDownTimerStop(aclConn); |
| } |
| |
| conn->next = mConns; |
| if (mConns) |
| mConns->prev = conn; |
| mConns = conn; |
| ret = true; |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| if (!ret && conn) |
| free(conn); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkEncrChangeInt |
| * USE: Called to change an ACL link's encr settings |
| * PARAMS: aclConn - the ACL link at hand |
| * isEncrypted - is it now encrypted? |
| * isMitmSafe - is it now MITM-safe? |
| * isKeyRefreshed - is a new key used for encryption |
| * RETURN: NONE |
| * NOTES: Call with mConnsLock held |
| */ |
| static void l2cAclLinkEncrChangeInt(struct l2cAclConn *aclConn, bool isEncrypted, bool isMitmSafe, |
| bool isKeyRefreshed) |
| { |
| struct l2cConn *iter = mConns, *conn; |
| struct l2cEncrState es; |
| bool changed; |
| |
| changed = (!aclConn->encrypted != !isEncrypted) || (!aclConn->mitmSafe != !isMitmSafe); |
| if (!isKeyRefreshed && !changed) { |
| logd("Unchanged encr call\n"); |
| return; |
| } |
| |
| aclConn->encrypted = isEncrypted; |
| aclConn->mitmSafe = isMitmSafe; |
| es.isEncr = isEncrypted; |
| es.isMitmSafe = isMitmSafe; |
| |
| /* at this point we're sure connection encryption status changed for non-key-refreshed, so |
| * inform everyone |
| */ |
| while (iter) { |
| conn = iter; |
| iter = iter->next; |
| |
| /* wrong link? */ |
| if (conn->acl != aclConn->handle) |
| continue; |
| |
| /* handle connections waiting for encryption */ |
| if (conn->state == L2C_CONN_STATE_ENCR_WAIT) { |
| |
| if (conn->psm) { |
| /* handle PSM connections */ |
| if(!l2cSvcWorkSchedulePsmAlloc(conn->psm, conn->handle)) |
| loge("Failed to re-enquque PSM alloc on encr\n"); |
| } else if (!l2cSvcWorkScheduleFixedChAlloc(conn->localCh, conn->handle)) { |
| /* handle FixedCh */ |
| loge("Failed to re-enquque fixed alloc on encr\n"); |
| } |
| } |
| |
| /* notify existing connections */ |
| if (conn->state == L2C_CONN_STATE_ESTABLISHED && |
| !l2cSvcWorkScheduleStateCall( |
| conn, isKeyRefreshed ? L2C_STATE_ENCR_KEY_REF : L2C_STATE_ENCR, &es, sizeof(es))) { |
| loge("Failed to send close state\n"); |
| } |
| } |
| } |
| |
| /* |
| * FUNCTION: l2cAclConnTeardownTimerCbk |
| * USE: Timer callback for ACL link down |
| * PARAMS: which - timer handle |
| * cbkData - the callback data |
| * RETURN: NONE |
| * NOTES: |
| */ |
| static void l2cAclConnTeardownTimerCbk(uniq_t which, uint64_t cbkData) |
| { |
| uniq_t aclHandle = (uniq_t)cbkData; |
| struct l2cAclConn *aclConn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindByHandle(aclHandle); |
| if (aclConn && aclConn->teardownTimer == which) { |
| logd("Link timer expired for acl "HCI_CONN_FMT"x. Closing\n", HCI_CONN_CONV(aclConn->conn)); |
| aclConn->state = L2C_ACL_STATE_TEARDOWN; |
| if (!l2cSvcWorkScheduleAclLinkClose(aclConn->handle)) |
| loge("Failed to schedule call to close acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| l2cAclLinkDownInt(aclConn, true); |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cAclDownTimerStart |
| * USE: Schedule a timer for ACL link shutdown |
| * PARAMS: aclConn - the connection |
| * RETURN: NONE |
| * NOTES: Call this when you're done with the link and you're the last one. |
| * Call with mConnsLock held |
| */ |
| static void l2cAclDownTimerStart(struct l2cAclConn *aclConn) |
| { |
| if (aclConn->teardownTimer) |
| timerCancel(aclConn->teardownTimer); |
| |
| aclConn->teardownTimer = timerSet(L2C_ACL_LINK_TIMOUT, l2cAclConnTeardownTimerCbk, aclConn->handle); |
| if (!aclConn->teardownTimer) |
| loge("Failed to set teardown timer for acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| } |
| |
| /* |
| * FUNCTION: l2cAclDownTimerStop |
| * USE: Remove a timer for ACL link shutdown |
| * PARAMS: aclConn - the connection |
| * RETURN: NONE |
| * NOTES: Call this when you start using a link |
| * Call with mConnsLock held |
| */ |
| static void l2cAclDownTimerStop(struct l2cAclConn *aclConn) |
| { |
| if (aclConn->teardownTimer) |
| timerCancel(aclConn->teardownTimer); |
| aclConn->teardownTimer = 0; |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkUp |
| * USE: Called when a new ACL link comes up |
| * PARAMS: aclConn - the conection ID |
| * peerAddr - the peer address |
| * selfAddr - whom we pretended to be when we connected |
| * isMaster - are we the master on the link? |
| * isEncrypted - is link encrypted? |
| * isMitmSafe - is link MITM safe? |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cAclLinkUp(hci_conn_t aclConnID, const struct bt_addr *peerAddr, const struct bt_addr *selfAddr, bool isMaster, bool isEncrypted, bool isMitmSafe) |
| { |
| struct l2cAclConn *aclConn; |
| struct l2cConn *connIter; |
| struct l2cFixedChSvc *svc; |
| bool inUse = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| if (l2cAclConnFindById(aclConnID)) |
| loge("Duplicate ID up (acl "HCI_CONN_FMT"x)\n", HCI_CONN_CONV(aclConnID)); |
| else { |
| /* TODO(mcchou): if including "hardware/bluetooth.h", complilatino fails with error |
| * aapiGatt.h:4:32: fatal error: hardware/bluetooth.h: No such file or directory |
| * As an workaround, we marked out the following function call to the userspace for now. |
| * aapiAclStateChanged(0, peerAddr, true); |
| */ |
| aclConn = l2cAclConnFindByAddr(peerAddr); |
| if (aclConn) { |
| if (aclConn->state == L2C_ACL_STATE_PENDING) { |
| aclConn->openTimer = 0; |
| logd("Us-initiated link up (acl "HCI_CONN_FMT"x)\n", HCI_CONN_CONV(aclConnID)); |
| } else { |
| loge("Duplicate link up with same address and different acl (old "HCI_CONN_FMT"x new "HCI_CONN_FMT"x)." |
| " Ignored.\n", HCI_CONN_CONV(aclConn->conn), HCI_CONN_CONV(aclConnID)); |
| goto out; |
| } |
| } else { |
| logd("Remote-initiated link up acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConnID)); |
| aclConn = (struct l2cAclConn*)calloc(1, sizeof(struct l2cAclConn)); |
| if (aclConn) { |
| aclConn->handle = uniqGetNext(); |
| aclConn->prev = NULL; |
| aclConn->reassSg = NULL; |
| aclConn->next = mAclConns; |
| aclConn->sigMtu = L2C_MIN_SIG_MTU; |
| if (mAclConns) |
| mAclConns->prev = aclConn; |
| mAclConns = aclConn; |
| } else { |
| loge("Failed to allocate a new l2cAclConn for acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConnID)); |
| goto out; |
| } |
| } |
| memcpy(&aclConn->peerAddr, peerAddr, sizeof(aclConn->peerAddr)); |
| memcpy(&aclConn->selfAddr, selfAddr, sizeof(aclConn->selfAddr)); |
| aclConn->ident = 1; |
| aclConn->teardownTimer = 0; |
| aclConn->isMaster = isMaster ? 1 : 0; |
| aclConn->connectionlessMtu = L2C_MIN_CONNECTIONLESS_MTU; |
| aclConn->connUpdtRequestIdent = 0; |
| aclConn->nextChan = L2C_NUM_FIXED_CHANNELS; |
| aclConn->conn = aclConnID; |
| aclConn->encrypted = isEncrypted; |
| aclConn->mitmSafe = isMitmSafe; |
| |
| /* send an info req if EDR */ |
| if (BT_ADDR_IS_EDR(*peerAddr)) { |
| aclConn->state = L2C_ACL_STATE_CFG; |
| aclConn->reqIdent = l2cSigSendInfoReq(aclConn, L2C_SIG_INFO_TYPE_UNCONN_MTU); |
| if (!aclConn->reqIdent) { |
| aclConn->state = L2C_ACL_STATE_ESTABLISHED; |
| logw("Failed to send req for connectionless mtu for acl "HCI_CONN_FMT"x. It will not be known\n", HCI_CONN_CONV(aclConn->conn)); |
| } |
| } else |
| aclConn->state = L2C_ACL_STATE_ESTABLISHED; |
| |
| /* handle all pending-open connections */ |
| connIter = mConns; |
| while (connIter){ |
| uint8_t ident = 0; |
| uint16_t localCh; |
| struct l2cConn *conn = connIter; |
| connIter = connIter->next; |
| |
| if (conn->acl != aclConn->handle) /* not for this ACL link */ |
| continue; |
| |
| if (conn->state != L2C_CONN_STATE_CONN_WAIT) { /* wrong state? */ |
| loge("L2C connection for new acl "HCI_CONN_FMT"x not in wait state on link up! Dropping\n", HCI_CONN_CONV(aclConnID)); |
| /* There is no sure way to delete this - we have no idea what state it is in. Do our best to not make it worse and do nothing */ |
| l2cConnStructDelete(conn); |
| continue; |
| } |
| |
| if (!conn->psm) { /* FixedCh */ |
| inUse = true; |
| l2cChannelReadyForData(aclConn, conn); |
| continue; |
| } |
| |
| /* PSM connection */ |
| localCh = l2cAclFreeCid(aclConn); |
| if (localCh) { |
| ident = l2cSigSendConnectionReq(aclConn, conn->psm, localCh); |
| conn->localCh = localCh; |
| } |
| |
| if (ident) |
| inUse = true; |
| else { |
| loge("Failed to request opening of a psm channel\n"); |
| if (!l2cSvcWorkScheduleStateCall(conn, L2C_STATE_CLOSED, NULL, 0)) |
| loge("Failed to send close state\n"); |
| l2cConnStructDelete(conn); |
| continue; |
| } |
| conn->state = L2C_CONN_STATE_TX_OPEN_TXED; |
| conn->ident = ident; |
| l2cConnTimer(conn, L2C_L2CAP_OPEN_TIMEOUT); |
| } |
| |
| /* Bring up all FixedCh services */ |
| pthread_mutex_lock(&mSvcsLock); |
| svc = mFixedSvcs; |
| while (svc) { |
| if (!svc->beingRemoved) { |
| /* See if a conn is already pending. If so we'll not instantiate the service. |
| * This may seem strange, but unlike PSM world, here we cannot have two FixedCh |
| * connections at the same time */ |
| struct l2cConn *conn = mConns; |
| while (conn && (conn->acl != aclConn->handle || conn->psm || conn->localCh != svc->chan)) |
| conn = conn->next; |
| |
| if (conn) { |
| logi("Not initing FixedCh %d svc since connection is pending\n", svc->chan); |
| svc = svc->next; |
| continue; |
| } |
| |
| conn = l2cNewConnStruct(L2C_CONN_STATE_RX_OPEN_SVC, aclConn->handle, svc->chan, svc->chan, 0, 0, false); |
| if (!conn) { |
| logw("Not initing FixedCh %d svc - cannot alloc conn struct\n", svc->chan); |
| svc = svc->next; |
| continue; |
| } |
| |
| conn->inUse = 0; /* until actually used */ |
| if (l2cSvcWorkScheduleFixedChAlloc(svc->chan, conn->handle)) { |
| conn->next = mConns; |
| if (mConns) |
| mConns->prev = conn; |
| mConns = conn; |
| } else { |
| loge("Failed to schedule FixedCh alloc for ch %d\n", svc->chan); |
| free(conn); |
| } |
| } |
| svc = svc->next; |
| } |
| pthread_mutex_unlock(&mSvcsLock); |
| |
| if (!inUse) { |
| l2cAclDownTimerStart(aclConn); |
| aclConn->teardownTimer = timerSet(L2C_ACL_LINK_TIMOUT, l2cAclConnTeardownTimerCbk, aclConn->handle); |
| if (!aclConn->teardownTimer) |
| loge("Failed to set teardown timer for acl "HCI_CONN_FMT"x\n", HCI_CONN_CONV(aclConn->conn)); |
| } |
| } |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkParamsChange |
| * USE: Called when LE ACL link params change |
| * PARAMS: aclConnID - the conection ID |
| * interval - the new interval |
| * latency - the new latency |
| * timeout - the new timeout |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cAclLinkParamsChange(hci_conn_t aclConnID, bool success, uint16_t interval, uint16_t latency, uint16_t timeout) |
| { |
| struct l2cAclConn *conn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cAclConnFindById(aclConnID); |
| if (!conn) |
| loge("Unable to find acl "HCI_CONN_FMT"x. Dropping params change notification\n", HCI_CONN_CONV(aclConnID)); |
| else if (!conn->isMaster) |
| logi("Got update for acl "HCI_CONN_FMT"x: {%d,%d,%d}\n", HCI_CONN_CONV(aclConnID), interval, latency, timeout); |
| else if (conn->connUpdtRequestIdent) { |
| if (!l2cSigSendConnUpdateRsp(conn, conn->connUpdtRequestIdent, success ? L2C_SIG_CONN_UPDT_OK : L2C_SIG_CONN_UPDT_NO)) |
| loge("Failed to sent connection update response frame\n"); |
| conn->connUpdtRequestIdent = 0; |
| } else if (conn->connUpdtDoneCbk) { |
| if (!l2cSvcWorkScheduleConnUpdtCbk(conn->connUpdtDoneCbk, conn->connUpdtDoneCbkData, success)) |
| loge("Failed to schedule connection update callback\n"); |
| conn->connUpdtDoneCbk = NULL; |
| } |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkDownInt |
| * USE: Called when an ACL link goes down or is about to go down |
| * Shuts down all our state for said link. |
| * PARAMS: aclConn - the ACL connection struct |
| * RETURN: NONE |
| * NOTES: call withmConnsLock held |
| */ |
| static void l2cAclLinkDownInt(struct l2cAclConn *aclConn, bool tellOtherSide) |
| { |
| struct l2cConn *conn = mConns, *t; |
| |
| while (conn && conn->acl == aclConn->handle) { |
| t = conn; |
| conn = conn->next; |
| l2cConnRequestClose(aclConn, t, tellOtherSide); |
| } |
| l2cAclConnStructDelete(aclConn); |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkDown |
| * USE: Called when a new ACL link goes down |
| * PARAMS: aclConn - the conection ID |
| * reason - the hci reason code |
| * RETURN: NONEI |
| * NOTES: |
| */ |
| void l2cAclLinkDown(hci_conn_t aclConnID, uint8_t reason) |
| { |
| struct l2cAclConn *aclConn; |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindById(aclConnID); |
| if (aclConn) { |
| /* TODO(mcchou): if including "hardware/bluetooth.h", complilatino fails with error |
| * aapiGatt.h:4:32: fatal error: hardware/bluetooth.h: No such file or directory |
| * As an workaround, we marked out the following function call to the userspace for now. |
| * aapiAclStateChanged(0, &aclConn->peerAddr, false); |
| */ |
| logd("Link acl "HCI_CONN_FMT"x actually down. Handling\n", HCI_CONN_CONV(aclConn->conn)); |
| l2cAclLinkDownInt(aclConn, false); |
| } else |
| logd("Link acl "HCI_CONN_FMT"x not found. Down ignored.\n", HCI_CONN_CONV(aclConnID)); |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkEncrChange |
| * USE: Called when a new ACL encryption changed |
| * PARAMS: aclConnID - the conection ID |
| * isEncrypted - is link now encrypted? |
| * isMitmSafe - is link now MITM-safe? |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cAclLinkEncrChange(hci_conn_t aclConnID, bool isEncrypted, bool isMitmSafe) |
| { |
| struct l2cAclConn *aclConn; |
| |
| logd("l2cAclLinkEncrChange(acl "HCI_CONN_FMT"x, %d, %d)\n", HCI_CONN_CONV(aclConnID), isEncrypted, isMitmSafe); |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindById(aclConnID); |
| if (aclConn) { |
| logd("Link acl "HCI_CONN_FMT"x encr change to %d,%d. Handling\n", HCI_CONN_CONV(aclConn->conn), isEncrypted,isMitmSafe); |
| l2cAclLinkEncrChangeInt(aclConn, isEncrypted, isMitmSafe, false); |
| } else |
| logd("Link acl "HCI_CONN_FMT"x not found. Encr change ignored.\n", HCI_CONN_CONV(aclConnID)); |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cAclLeLtkRequest |
| * USE: Called when an Le connection needs a key |
| * PARAMS: aclConnID - the conection ID |
| * randomNum - provided by other side |
| * diversifier - provided by other side |
| * RETURN: false on immediate failure |
| * NOTES: |
| */ |
| bool l2cAclLeLtkRequest(hci_conn_t aclConnID, uint64_t randomNum, uint16_t diversifier) |
| { |
| struct l2cKeyReqState keyReq = {.rand = randomNum, .ediv = diversifier, }; |
| L2capDefaultSecurityManagerCbk defaultSecMgr; |
| struct l2cAclConn *aclConn; |
| struct l2cConn *conn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mSecMgrLock); |
| defaultSecMgr = mDefaultSecMgr; |
| pthread_mutex_unlock(&mSecMgrLock); |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindById(aclConnID); |
| if ((!aclConn || !aclConn->securityManager) && defaultSecMgr && !l2cSvcWorkScheduleDefaultSmCall(defaultSecMgr, aclConnID, randomNum, diversifier)) |
| logi("Default security manager call failed for acl "HCI_CONN_FMT"x. LTK req ignored.\n", HCI_CONN_CONV(aclConnID)); |
| else if (!aclConn) |
| logw("Link acl "HCI_CONN_FMT"x not found. LTK req ignored.\n", HCI_CONN_CONV(aclConnID)); |
| else if (!aclConn->securityManager) |
| logi("Link acl "HCI_CONN_FMT"x has no assigned security manager. LTK req ignored.\n", HCI_CONN_CONV(aclConnID)); |
| else { |
| conn = l2cConnFindByHandle(aclConn->securityManager); |
| if (!conn) |
| logi("Link acl "HCI_CONN_FMT"x's assigned security manager does nt exist anymore. LTK req ignored.\n", HCI_CONN_CONV(aclConnID)); |
| else |
| ret = l2cSvcWorkScheduleStateCall(conn, L2C_STATE_KEY_REQ, &keyReq, sizeof(keyReq)); |
| } |
| |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiLeProvideLtk |
| * USE: Called by SM - provide key |
| * PARAMS: handle - connection handle |
| * key - key to use or NULL if none |
| * RETURN: false on error |
| * NOTES: proper hci reply command will be called |
| */ |
| bool l2cApiLeProvideLtk(l2c_handle_t handle, const uint8_t *key) |
| { |
| struct l2cAclConn *aclConn = NULL; |
| struct l2cConn *conn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| conn = l2cConnFindByHandle(handle); |
| if (conn) |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| |
| if (!conn) |
| logw("L2C conn "HANDLEFMT"x not found. LTK reply ignored.\n", HANDLECNV(handle)); |
| else if (!aclConn) |
| logw("L2C conn "HANDLEFMT"x's acl conn not found. LTK reply ignored. Also, this is bad!\n", HANDLECNV(handle)); |
| else |
| ret = hciLeProvideLtk(aclConn->conn, key); |
| |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cAclLinkEncrKeyRefresh |
| * USE: Called when a ACL encryption is refreshed with a new key |
| * PARAMS: aclConnID - the conection ID |
| * isEncrypted - is link now encrypted? |
| * isMitmSafe - is link now MITM-safe? |
| * RETURN: NONE |
| * NOTES: |
| */ |
| void l2cAclLinkEncrKeyRefresh(hci_conn_t aclConnID, bool isEncrypted, bool isMitmSafe) |
| { |
| struct l2cAclConn *aclConn; |
| |
| logd("l2cAclLinkEncrKeyRefresh(acl "HCI_CONN_FMT"x, %d, %d)\n", HCI_CONN_CONV(aclConnID), isEncrypted, isMitmSafe); |
| |
| pthread_mutex_lock(&mConnsLock); |
| aclConn = l2cAclConnFindById(aclConnID); |
| if (aclConn) { |
| logd("Link acl "HCI_CONN_FMT"x encr key refreshed and changed to %d,%d. Handling\n", HCI_CONN_CONV(aclConn->conn), isEncrypted,isMitmSafe); |
| l2cAclLinkEncrChangeInt(aclConn, isEncrypted, isMitmSafe, true); |
| } else |
| logd("Link acl "HCI_CONN_FMT"x not found. Encr key refreshed ignored.\n", HCI_CONN_CONV(aclConnID)); |
| pthread_mutex_unlock(&mConnsLock); |
| } |
| |
| /* |
| * FUNCTION: l2cApiLeSetSecurityManagerForAclConn |
| * USE: Called by a security manager to become one for a given acl connection |
| * PARAMS: handle - connection handle |
| * RETURN: true if no security manager already existed for a given acl |
| * connection and the requestor became one, requestor already |
| * was the security manager for this acl connection, or the previous |
| * security manager was no longer in existence (l2c conn had been closed) |
| * NOTES: |
| */ |
| bool l2cApiLeSetSecurityManagerForAclConn(l2c_handle_t handle) |
| { |
| struct l2cConn *conn; |
| struct l2cAclConn *aclConn; |
| bool ret = false; |
| |
| pthread_mutex_lock(&mConnsLock); |
| |
| conn = l2cConnFindByHandle(handle); |
| if (!conn) { |
| loge("Connection not found\n"); |
| goto out; |
| } |
| |
| aclConn = l2cAclConnFindByHandle(conn->acl); |
| if (!aclConn) { |
| loge("ACL link not found\n"); |
| goto out; |
| } |
| |
| if (!aclConn->securityManager || !l2cConnFindByHandle(aclConn->securityManager)) |
| aclConn->securityManager = handle; |
| |
| ret = (aclConn->securityManager == handle); |
| |
| out: |
| pthread_mutex_unlock(&mConnsLock); |
| return ret; |
| } |
| |
| /* |
| * FUNCTION: l2cApiLeSetDefaultSecurityManager |
| * USE: Called by a security manager to become one by default, is no per-connection |
| * security manager has been set |
| * PARAMS: cbk - the function to call when a key is needed or NULL |
| * RETURN: none |
| * NOTES: This sets the security manager to be used when no l2c connection yet exists |
| * on an hci acl connection. This means that sm had not yet had a chance to call |
| * l2cApiLeSetSecurityManagerForAclConn(). This can happen if someone connects |
| * and immediately tries to encrypt, with no prior traffic. This is allowed, |
| * but no traffic means that SM has not been opened, and thus has no l2c conn |
| * associated with it. No l2c conn means we have no instance to send the key |
| * state to. This this callback is used. If an l2c conn exists that has called |
| * l2cApiLeSetSecurityManagerForAclConn, it is used for security management |
| * instead. |
| */ |
| void l2cApiLeSetDefaultSecurityManager(L2capDefaultSecurityManagerCbk cbk) |
| { |
| /* |
| * Since we actually pass the callback pointer in the work items on the work queue, |
| * we need to make sure none escape this change func call. To do this we use the |
| * ability to "flush" the work queue - wait till all current items finish. We do it |
| * twice. Once before changing and once after. The first case makes sure that change |
| * to NULL indeed no longer calls this callback after this func ends. Second makes |
| * sure that a change to a pointer take effect before this func returns. |
| */ |
| pthread_mutex_lock(&mSecMgrLock); |
| l2cSvcWorkScheduleFlush(); |
| mDefaultSecMgr = cbk; |
| l2cSvcWorkScheduleFlush(); |
| pthread_mutex_unlock(&mSecMgrLock); |
| } |
| |
| /* |
| * FUNCTION: l2cUtilFcs |
| * USE: Calculate an L2CAP FCS (CRC-16 with poly 0x8005) for some data |
| * PARAMS: fcs - input fcs |
| * data - the data |
| * len - the length of said data |
| * RETURN: the fcs |
| * NOTES: |
| */ |
| static uint16_t l2cUtilFcs(uint16_t fcs, const void *data, uint32_t len) |
| { |
| static const uint16_t crctab[] = { |
| 0x0000, 0xC0C1, 0xC181, 0x0140, 0xC301, 0x03C0, 0x0280, 0xC241, 0xC601, 0x06C0, 0x0780, 0xC741, 0x0500, 0xC5C1, 0xC481, |
| 0x0440, 0xCC01, 0x0CC0, 0x0D80, 0xCD41, 0x0F00, 0xCFC1, 0xCE81, 0x0E40, 0x0A00, 0xCAC1, 0xCB81, 0x0B40, 0xC901, 0x09C0, |
| 0x0880, 0xC841, 0xD801, 0x18C0, 0x1980, 0xD941, 0x1B00, 0xDBC1, 0xDA81, 0x1A40, 0x1E00, 0xDEC1, 0xDF81, 0x1F40, 0xDD01, |
| 0x1DC0, 0x1C80, 0xDC41, 0x1400, 0xD4C1, 0xD581, 0x1540, 0xD701, 0x17C0, 0x1680, 0xD641, 0xD201, 0x12C0, 0x1380, 0xD341, |
| 0x1100, 0xD1C1, 0xD081, 0x1040, 0xF001, 0x30C0, 0x3180, 0xF141, 0x3300, 0xF3C1, 0xF281, 0x3240, 0x3600, 0xF6C1, 0xF781, |
| 0x3740, 0xF501, 0x35C0, 0x3480, 0xF441, 0x3C00, 0xFCC1, 0xFD81, 0x3D40, 0xFF01, 0x3FC0, 0x3E80, 0xFE41, 0xFA01, 0x3AC0, |
| 0x3B80, 0xFB41, 0x3900, 0xF9C1, 0xF881, 0x3840, 0x2800, 0xE8C1, 0xE981, 0x2940, 0xEB01, 0x2BC0, 0x2A80, 0xEA41, 0xEE01, |
| 0x2EC0, 0x2F80, 0xEF41, 0x2D00, 0xEDC1, 0xEC81, 0x2C40, 0xE401, 0x24C0, 0x2580, 0xE541, 0x2700, 0xE7C1, 0xE681, 0x2640, |
| 0x2200, 0xE2C1, 0xE381, 0x2340, 0xE101, 0x21C0, 0x2080, 0xE041, 0xA001, 0x60C0, 0x6180, 0xA141, 0x6300, 0xA3C1, 0xA281, |
| 0x6240, 0x6600, 0xA6C1, 0xA781, 0x6740, 0xA501, 0x65C0, 0x6480, 0xA441, 0x6C00, 0xACC1, 0xAD81, 0x6D40, 0xAF01, 0x6FC0, |
| 0x6E80, 0xAE41, 0xAA01, 0x6AC0, 0x6B80, 0xAB41, 0x6900, 0xA9C1, 0xA881, 0x6840, 0x7800, 0xB8C1, 0xB981, 0x7940, 0xBB01, |
| 0x7BC0, 0x7A80, 0xBA41, 0xBE01, 0x7EC0, 0x7F80, 0xBF41, 0x7D00, 0xBDC1, 0xBC81, 0x7C40, 0xB401, 0x74C0, 0x7580, 0xB541, |
| 0x7700, 0xB7C1, 0xB681, 0x7640, 0x7200, 0xB2C1, 0xB381, 0x7340, 0xB101, 0x71C0, 0x7080, 0xB041, 0x5000, 0x90C1, 0x9181, |
| 0x5140, 0x9301, 0x53C0, 0x5280, 0x9241, 0x9601, 0x56C0, 0x5780, 0x9741, 0x5500, 0x95C1, 0x9481, 0x5440, 0x9C01, 0x5CC0, |
| 0x5D80, 0x9D41, 0x5F00, 0x9FC1, 0x9E81, 0x5E40, 0x5A00, 0x9AC1, 0x9B81, 0x5B40, 0x9901, 0x59C0, 0x5880, 0x9841, 0x8801, |
| 0x48C0, 0x4980, 0x8941, 0x4B00, 0x8BC1, 0x8A81, 0x4A40, 0x4E00, 0x8EC1, 0x8F81, 0x4F40, 0x8D01, 0x4DC0, 0x4C80, 0x8C41, |
| 0x4400, 0x84C1, 0x8581, 0x4540, 0x8701, 0x47C0, 0x4680, 0x8641, 0x8201, 0x42C0, 0x4380, 0x8341, 0x4100, 0x81C1, 0x8081 |
| }; |
| |
| const uint8_t *ptr = (const uint8_t*)data; |
| |
| while (len--) |
| fcs = (fcs >> 8) ^ crctab[(fcs & 0xFF) ^ *ptr++]; |
| |
| return fcs; |
| } |
| |
| /* |
| * FUNCTION: l2cUtilFcsSg |
| * USE: Calculate an L2CAP FCS (CRC-16 with poly 0x8005) for an SG |
| * PARAMS: fcs - input fcs |
| * s - the sg |
| * RETURN: the fcs |
| * NOTES: |
| */ |
| static uint16_t l2cUtilFcsSg(uint16_t fcs, sg s) |
| { |
| void *iter; |
| |
| for (iter = sgIterStart(s); iter; iter = sgIterAdvance(iter)) |
| fcs = l2cUtilFcs(fcs, sgIterCurData(iter), sgIterCurLen(iter)); |
| |
| return fcs; |
| } |
| |
| |