void smpSendPairingFailed(smpCcb_t *pCcb, uint8_t reason)
{
uint8_t *pPacket;
uint8_t *p;
if ((pPacket = smpMsgAlloc(L2C_PAYLOAD_START + SMP_PAIR_FAIL_LEN)) != NULL)
{
p = pPacket + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, SMP_CMD_PAIR_FAIL);
UINT8_TO_BSTREAM(p, reason);
smpSendPkt(pCcb, pPacket);
}
}
void smprActSendPairRandom(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
uint8_t *pPkt;
uint8_t *p;
uint8_t encKeyLen;
/* get max STK length */
encKeyLen = (pCcb->pairReq[SMP_MAXKEY_POS] < pCcb->pairRsp[SMP_MAXKEY_POS]) ?
pCcb->pairReq[SMP_MAXKEY_POS] : pCcb->pairRsp[SMP_MAXKEY_POS];
/* store STK and adjust based on max key length */
memcpy(pCcb->pScr->buf.b3, pMsg->aes.pCiphertext, encKeyLen);
memset((pCcb->pScr->buf.b3 + encKeyLen), 0, (SMP_KEY_LEN - encKeyLen));
/* start smp response timer */
smpStartRspTimer(pCcb);
/* allocate packet buffer and send pairing random packet */
if ((pPkt = smpMsgAlloc(SMP_PAIR_RAND_LEN + L2C_PAYLOAD_START)) != NULL)
{
/* build packet */
p = pPkt + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, SMP_CMD_PAIR_RAND);
memcpy(p, pCcb->pScr->buf.b4, SMP_RAND_LEN);
/* send packet */
smpSendPkt(pCcb, pPkt);
}
}
void HciLeSetConnCteTxParamsCmd(uint16_t connHandle, uint8_t cteTypeBits, uint8_t switchPatternLen,
uint8_t *pAntennaIDs)
{
uint8_t *pBuf;
uint8_t *p;
if ((pBuf = hciCmdAlloc(HCI_OPCODE_LE_SET_CONN_CTE_TX_PARAMS,
HCI_LEN_LE_SET_CONN_CTE_TX_PARAMS(switchPatternLen))) != NULL)
{
p = pBuf + HCI_CMD_HDR_LEN;
UINT16_TO_BSTREAM(p, connHandle);
UINT8_TO_BSTREAM(p, cteTypeBits);
UINT8_TO_BSTREAM(p, switchPatternLen);
memcpy(p, pAntennaIDs, switchPatternLen);
hciCmdSend(pBuf);
}
}
void HciLeConnCteReqEnableCmd(uint16_t connHandle, uint8_t enable, uint16_t cteReqInt,
uint8_t reqCteLen, uint8_t reqCteType)
{
uint8_t *pBuf;
uint8_t *p;
if ((pBuf = hciCmdAlloc(HCI_OPCODE_LE_CONN_CTE_REQ_ENABLE,
HCI_LEN_LE_CONN_CTE_REQ_ENABLE)) != NULL)
{
p = pBuf + HCI_CMD_HDR_LEN;
UINT16_TO_BSTREAM(p, connHandle);
UINT8_TO_BSTREAM(p, enable);
UINT16_TO_BSTREAM(p, cteReqInt);
UINT8_TO_BSTREAM(p, reqCteLen);
UINT8_TO_BSTREAM(p, reqCteType);
hciCmdSend(pBuf);
}
}
void L2cDmConnUpdateRsp(uint8_t identifier, uint16_t handle, uint16_t result)
{
uint8_t *pPacket;
uint8_t *p;
/* allocate msg buffer */
if ((pPacket = l2cMsgAlloc(L2C_SIG_PKT_BASE_LEN + L2C_SIG_CONN_UPDATE_RSP_LEN)) != NULL)
{
/* build message */
p = pPacket + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, L2C_SIG_CONN_UPDATE_RSP); /* command code */
UINT8_TO_BSTREAM(p, identifier); /* identifier */
UINT16_TO_BSTREAM(p, L2C_SIG_CONN_UPDATE_RSP_LEN); /* parameter length */
UINT16_TO_BSTREAM(p, result); /* result */
/* send packet */
L2cDataReq(L2C_CID_LE_SIGNALING, handle, (L2C_SIG_HDR_LEN + L2C_SIG_CONN_UPDATE_RSP_LEN), pPacket);
}
}
static uint8_t lhciVsPackScanReportEvt(uint8_t *pBuf, const LlScanReportInd_t *pEvt)
{
const uint8_t len = LHCI_LEN_VS_SUBEVT_SCAN_REPORT;
UINT8_TO_BSTREAM(pBuf, pEvt->peerAddrType);
BDA64_TO_BSTREAM(pBuf, pEvt->peerAddr);
BDA64_TO_BSTREAM(pBuf, pEvt->peerRpa);
return len;
}
void smprActSendSecurityReq(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
uint8_t *pPkt;
uint8_t *p;
/* start smp response timer */
smpStartRspTimer(pCcb);
/* allocate packet buffer */
if ((pPkt = smpMsgAlloc(SMP_SECURITY_REQ_LEN + L2C_PAYLOAD_START)) != NULL)
{
/* build packet */
p = pPkt + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, SMP_CMD_SECURITY_REQ);
UINT8_TO_BSTREAM(p, pMsg->dm.securityReq.auth);
/* send packet */
smpSendPkt(pCcb, pPkt);
}
}
void smprActSendPairRsp(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
uint8_t *pPkt;
uint8_t *p;
uint8_t oob;
uint8_t display;
/* build packet to pairing response buffer in ccb */
p = pCcb->pairRsp;
UINT8_TO_BSTREAM(p, SMP_CMD_PAIR_RSP);
UINT8_TO_BSTREAM(p, pSmpCfg->ioCap);
UINT8_TO_BSTREAM(p, pMsg->dm.pair.oob);
UINT8_TO_BSTREAM(p, pMsg->dm.pair.auth);
UINT8_TO_BSTREAM(p, pSmpCfg->maxKeyLen);
UINT8_TO_BSTREAM(p, pMsg->dm.pair.iKeyDist);
UINT8_TO_BSTREAM(p, pMsg->dm.pair.rKeyDist);
/* process pairing request and response data */
if (smpCb.procPairing(pCcb, &oob, &display))
{
/* set next expected packet */
if ((pCcb->pairReq[SMP_AUTHREQ_POS] & pMsg->dm.pair.auth & SMP_AUTH_SC_FLAG) == SMP_AUTH_SC_FLAG)
{
pCcb->nextCmdCode = SMP_CMD_PUBLIC_KEY;
}
else
{
pCcb->nextCmdCode = SMP_CMD_PAIR_CNF;
}
/* start smp response timer */
smpStartRspTimer(pCcb);
/* send pairing response; allocate packet buffer */
if ((pPkt = smpMsgAlloc(SMP_PAIR_RSP_LEN + L2C_PAYLOAD_START)) != NULL)
{
/* build packet from pairing response buffer */
memcpy(pPkt + L2C_PAYLOAD_START, pCcb->pairRsp, SMP_PAIR_RSP_LEN);
/* send packet */
smpSendPkt(pCcb, pPkt);
}
/* request authentication data */
smpCb.procAuthReq(pCcb, oob, display);
}
}
void AttcMtuReq(dmConnId_t connId, uint16_t mtu)
{
attcPktParam_t *pPkt;
uint8_t *p;
/* allocate packet and parameter buffer */
if ((pPkt = attMsgAlloc(ATT_MTU_REQ_BUF_LEN)) != NULL)
{
/* set length */
pPkt->len = ATT_MTU_REQ_LEN;
/* build packet */
p = (uint8_t *) pPkt + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, ATT_PDU_MTU_REQ);
UINT16_TO_BSTREAM(p, mtu);
/* send message */
attcSendMsg(connId, 0, ATTC_MSG_API_MTU, pPkt, FALSE);
}
}
void AttcWriteReq(dmConnId_t connId, uint16_t handle, uint16_t valueLen, uint8_t *pValue)
{
attcPktParam_t *pPkt;
uint8_t *p;
/* allocate packet and parameter buffer */
if ((pPkt = attMsgAlloc(ATT_WRITE_REQ_BUF_LEN + valueLen)) != NULL)
{
/* set length */
pPkt->len = ATT_WRITE_REQ_LEN + valueLen;
/* build packet */
p = (uint8_t *) pPkt + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, ATT_PDU_WRITE_REQ);
UINT16_TO_BSTREAM(p, handle);
memcpy(p, pValue, valueLen);
/* send message */
attcSendMsg(connId, handle, ATTC_MSG_API_WRITE, pPkt, FALSE);
}
}
void AttcFindInfoReq(dmConnId_t connId, uint16_t startHandle, uint16_t endHandle, bool_t continuing)
{
attcPktParam_t *pPkt;
uint8_t *p;
/* allocate packet and parameter buffer */
if ((pPkt = attMsgAlloc(ATT_FIND_INFO_REQ_BUF_LEN)) != NULL)
{
/* set parameters */
pPkt->len = ATT_FIND_INFO_REQ_LEN;
pPkt->h.startHandle = startHandle;
pPkt->h.endHandle = endHandle;
/* build partial packet */
p = (uint8_t *) pPkt + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, ATT_PDU_FIND_INFO_REQ);
/* send message */
attcSendMsg(connId, startHandle, ATTC_MSG_API_FIND_INFO, pPkt, continuing);
}
}
void smpActSendPairCnf(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
uint8_t *pPkt;
uint8_t *p;
/* set next expected packet */
pCcb->nextCmdCode = (pCcb->initiator) ? SMP_CMD_PAIR_CNF : SMP_CMD_PAIR_RAND;
/* start smp response timer */
smpStartRspTimer(pCcb);
/* allocate packet buffer */
if ((pPkt = smpMsgAlloc(SMP_PAIR_CNF_LEN + L2C_PAYLOAD_START)) != NULL)
{
/* build packet */
p = pPkt + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, SMP_CMD_PAIR_CNF);
memcpy(p, pMsg->aes.pCiphertext, SMP_CONFIRM_LEN);
/* send packet */
smpSendPkt(pCcb, pPkt);
}
}
bool_t smpSendKey(smpCcb_t *pCcb, uint8_t keyDist)
{
uint8_t *pPkt;
uint8_t *p;
wsfMsgHdr_t *pHdr;
if (smpCb.lescSupported && pCcb->pScCcb->lescEnabled && pCcb->lastSentKey == 0)
{
dmSecKeyIndEvt_t keyInd;
/* pass LTK to app via DM */
if (DmConnRole(pCcb->connId) == DM_ROLE_MASTER)
{
keyInd.type = DM_KEY_PEER_LTK;
}
else
{
keyInd.type = DM_KEY_LOCAL_LTK;
}
keyInd.hdr.event = DM_SEC_KEY_IND;
keyInd.hdr.param = pCcb->connId;
keyInd.secLevel = smpGetScSecLevel(pCcb);
keyInd.keyData.ltk.ediv = 0;
memset(keyInd.keyData.ltk.rand, 0, SMP_RAND8_LEN);
Calc128Cpy(keyInd.keyData.ltk.key, pCcb->pScCcb->pLtk->ltk_t);
DmSmpCbackExec((dmEvt_t *)&keyInd);
pCcb->lastSentKey = SMP_CMD_MASTER_ID;
}
/* check if we're done sending keys */
if ((keyDist == 0) ||
(keyDist == SMP_KEY_DIST_ENC && pCcb->lastSentKey == SMP_CMD_MASTER_ID) ||
(keyDist <= (SMP_KEY_DIST_ENC | SMP_KEY_DIST_ID) && pCcb->lastSentKey == SMP_CMD_ID_ADDR_INFO) ||
(pCcb->lastSentKey == SMP_CMD_SIGN_INFO))
{
return TRUE;
}
/* if flow disabled return */
if (pCcb->flowDisabled)
{
return FALSE;
}
/* allocate packet buffer for largest packet size */
if ((pPkt = smpMsgAlloc(SMP_ENC_INFO_LEN + L2C_PAYLOAD_START)) != NULL)
{
p = pPkt + L2C_PAYLOAD_START;
/* determine next key to send */
if (pCcb->lastSentKey == 0 && (keyDist & SMP_KEY_DIST_ENC))
{
/* generate LTK, EDIV, and RAND */
smpGenerateLtk(pCcb);
/* send first part of LTK */
UINT8_TO_BSTREAM(p, SMP_CMD_ENC_INFO);
Calc128Cpy(p, pCcb->pScr->keyInd.keyData.ltk.key);
}
else if (pCcb->lastSentKey == SMP_CMD_ENC_INFO)
{
/* send second part of LTK */
UINT8_TO_BSTREAM(p, SMP_CMD_MASTER_ID);
UINT16_TO_BSTREAM(p, pCcb->pScr->keyInd.keyData.ltk.ediv);
memcpy(p, pCcb->pScr->keyInd.keyData.ltk.rand, SMP_RAND8_LEN);
}
else if ((keyDist & SMP_KEY_DIST_ID) &&
(pCcb->lastSentKey == 0 || pCcb->lastSentKey == SMP_CMD_MASTER_ID))
{
/* send first part of IRK */
UINT8_TO_BSTREAM(p, SMP_CMD_ID_INFO);
Calc128Cpy(p, DmSecGetLocalIrk());
}
else if (pCcb->lastSentKey == SMP_CMD_ID_INFO)
{
/* send second part of IRK */
UINT8_TO_BSTREAM(p, SMP_CMD_ID_ADDR_INFO);
UINT8_TO_BSTREAM(p, DM_ADDR_PUBLIC);
BDA_TO_BSTREAM(p, HciGetBdAddr());
}
else if ((keyDist & SMP_KEY_DIST_SIGN) &&
(pCcb->lastSentKey == 0 || pCcb->lastSentKey == SMP_CMD_ID_ADDR_INFO ||
pCcb->lastSentKey == SMP_CMD_MASTER_ID))
{
/* send SRK */
UINT8_TO_BSTREAM(p, SMP_CMD_SIGN_INFO);
Calc128Cpy(p, DmSecGetLocalCsrk());
}
else
{
/* should never get here */
WsfMsgFree(pPkt);
SMP_TRACE_WARN2("smpSendKey unexpected state keyDist:%d lastSentKey:%d", keyDist, pCcb->lastSentKey);
return TRUE;
}
/* set last sent key to command code */
pCcb->lastSentKey = pPkt[L2C_PAYLOAD_START];
/* send command packet */
smpSendPkt(pCcb, pPkt);
/* if flow not disabled set up to send next key */
if (!pCcb->flowDisabled)
{
if ((pHdr = WsfMsgAlloc(sizeof(wsfMsgHdr_t))) != NULL)
{
pHdr->event = SMP_MSG_INT_SEND_NEXT_KEY;
pHdr->param = pCcb->connId;
WsfMsgSend(smpCb.handlerId, pHdr);
}
}
}
return FALSE;
}
void attsProcExecWriteReq(attCcb_t *pCcb, uint16_t len, uint8_t *pPacket)
{
uint8_t *pBuf;
uint8_t *p;
attsPrepWrite_t *pPrep;
attsAttr_t *pAttr;
attsGroup_t *pGroup;
uint8_t err = ATT_SUCCESS;
pPacket += L2C_PAYLOAD_START + ATT_HDR_LEN;
/* if cancelling all prepared writes */
if (*pPacket == ATT_EXEC_WRITE_CANCEL)
{
/* free all queued buffers */
attsClearPrepWrites(pCcb);
}
/* else writing all prepared writes */
else if (*pPacket == ATT_EXEC_WRITE_ALL)
{
/* iterate over prepare write queue and verify offset and length */
for (pPrep = pCcb->prepWriteQueue.pHead; pPrep != NULL; pPrep = pPrep->pNext)
{
/* find attribute */
if ((pAttr = attsFindByHandle(pPrep->handle, &pGroup)) != NULL)
{
/* verify offset */
if (pPrep->offset > pAttr->maxLen)
{
err = ATT_ERR_OFFSET;
}
/* verify write length with offset */
else if ((pPrep->writeLen + pPrep->offset) > pAttr->maxLen)
{
err = ATT_ERR_LENGTH;
}
if (err)
{
/* verification failed; discard all prepared writes */
attsClearPrepWrites(pCcb);
break;
}
}
}
/* if length and offset checks ok then write all buffers in queue */
if (err == ATT_SUCCESS)
{
/* for each buffer */
while ((pPrep = WsfQueueDeq(&pCcb->prepWriteQueue)) != NULL)
{
/* write buffer */
if ((err = attsExecPrepWrite(pCcb, pPrep)) != ATT_SUCCESS)
{
/* write failed; discard remaining prepared writes */
attsClearPrepWrites(pCcb);
}
/* free buffer */
WsfBufFree(pPrep);
}
}
}
/* else unknown operation */
else
{
err = ATT_ERR_INVALID_PDU;
}
/* send response or error response */
if (err)
{
attsErrRsp(pCcb->handle, ATT_PDU_EXEC_WRITE_REQ, 0, err);
}
else
{
if ((pBuf = attMsgAlloc(L2C_PAYLOAD_START + ATT_EXEC_WRITE_RSP_LEN)) != NULL)
{
/* build and send PDU */
p = pBuf + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, ATT_PDU_EXEC_WRITE_RSP);
L2cDataReq(L2C_CID_ATT, pCcb->handle, ATT_EXEC_WRITE_RSP_LEN, pBuf);
}
}
}
void attsProcPrepWriteReq(attCcb_t *pCcb, uint16_t len, uint8_t *pPacket)
{
uint8_t *pBuf;
uint8_t *p;
attsAttr_t *pAttr;
attsGroup_t *pGroup;
attsPrepWrite_t *pPrep;
uint16_t handle;
uint16_t offset;
uint16_t writeLen;
uint8_t err = ATT_SUCCESS;
/* parse handle and offset, calculate write length */
pPacket += L2C_PAYLOAD_START + ATT_HDR_LEN;
BSTREAM_TO_UINT16(handle, pPacket);
BSTREAM_TO_UINT16(offset, pPacket);
writeLen = len - ATT_PREP_WRITE_REQ_LEN; /* length of value being written */
/* find attribute */
if ((pAttr = attsFindByHandle(handle, &pGroup)) == NULL)
{
/* attribute not found */
err = ATT_ERR_HANDLE;
}
/* verify permissions */
else if ((err = attsPermissions(pCcb->connId, ATTS_PERMIT_WRITE,
handle, pAttr->permissions)) != ATT_SUCCESS)
{
/* err has been set; fail */
}
/* verify offset is allowed */
else if ((offset != 0) && ((pAttr->settings & ATTS_SET_ALLOW_OFFSET) == 0))
{
err = ATT_ERR_NOT_LONG;
}
/* verify write length, fixed length */
else if (((pAttr->settings & ATTS_SET_VARIABLE_LEN) == 0) &&
(writeLen != pAttr->maxLen))
{
err = ATT_ERR_LENGTH;
}
/* verify prepare write queue limit not reached */
else if (WsfQueueCount(&pCcb->prepWriteQueue) >= pAttCfg->numPrepWrites)
{
err = ATT_ERR_QUEUE_FULL;
}
/* allocate new buffer to hold prepared write */
else if ((pPrep = WsfBufAlloc(sizeof(attsPrepWrite_t) - 1 + writeLen)) == NULL)
{
err = ATT_ERR_RESOURCES;
}
else if ((pAttr->settings & ATTS_SET_WRITE_CBACK) &&
(pGroup->writeCback != NULL))
{
err = (*pGroup->writeCback)(pCcb->connId, handle, ATT_PDU_PREP_WRITE_REQ, 0, writeLen,
pPacket, pAttr);
}
if (err == ATT_SUCCESS)
{
/* copy data to new buffer and queue it */
pPrep->writeLen = writeLen;
pPrep->handle = handle;
pPrep->offset = offset;
memcpy(pPrep->packet, pPacket, writeLen);
WsfQueueEnq(&pCcb->prepWriteQueue, pPrep);
/* allocate response buffer */
if ((pBuf = attMsgAlloc(L2C_PAYLOAD_START + ATT_PREP_WRITE_RSP_LEN + writeLen)) != NULL)
{
/* build and send PDU */
p = pBuf + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, ATT_PDU_PREP_WRITE_RSP);
UINT16_TO_BSTREAM(p, handle);
UINT16_TO_BSTREAM(p, offset);
memcpy(p, pPacket, writeLen);
L2cDataReq(L2C_CID_ATT, pCcb->handle, (ATT_PREP_WRITE_RSP_LEN + writeLen), pBuf);
}
}
if (err)
{
attsErrRsp(pCcb->handle, ATT_PDU_PREP_WRITE_REQ, handle, err);
}
}
void attsProcWrite(attCcb_t *pCcb, uint16_t len, uint8_t *pPacket)
{
uint8_t *pBuf;
uint8_t *p;
attsAttr_t *pAttr;
attsGroup_t *pGroup;
uint8_t opcode;
uint16_t handle;
uint16_t writeLen;
uint8_t err = ATT_SUCCESS;
/* parse opcode handle, calculate write length */
pPacket += L2C_PAYLOAD_START;
BSTREAM_TO_UINT8(opcode, pPacket);
BSTREAM_TO_UINT16(handle, pPacket);
writeLen = len - ATT_WRITE_REQ_LEN;
/* find attribute */
if ((pAttr = attsFindByHandle(handle, &pGroup)) != NULL)
{
/* verify permissions */
if ((err = attsPermissions(pCcb->connId, ATTS_PERMIT_WRITE,
handle, pAttr->permissions)) != ATT_SUCCESS)
{
/* err has been set; fail */
}
/* verify write length, fixed length */
else if (((pAttr->settings & ATTS_SET_VARIABLE_LEN) == 0) &&
(writeLen != pAttr->maxLen))
{
err = ATT_ERR_LENGTH;
}
/* verify write length, variable length */
else if (((pAttr->settings & ATTS_SET_VARIABLE_LEN) != 0) &&
(writeLen > pAttr->maxLen))
{
err = ATT_ERR_LENGTH;
}
else
{
/* if write callback is desired */
if ((pAttr->settings & ATTS_SET_WRITE_CBACK) &&
(pGroup->writeCback != NULL))
{
err = (*pGroup->writeCback)(pCcb->connId, handle, opcode, 0, writeLen,
pPacket, pAttr);
}
/* else check if CCC */
else if ((pAttr->settings & ATTS_SET_CCC) && (attsCb.cccCback != NULL))
{
err = (*attsCb.cccCback)(pCcb->connId, ATT_METHOD_WRITE, handle, pPacket);
}
else
{
/* write attribute value */
memcpy(pAttr->pValue, pPacket, writeLen);
/* write the length if variable length attribute */
if ((pAttr->settings & ATTS_SET_VARIABLE_LEN) != 0)
{
*(pAttr->pLen) = writeLen;
}
}
/* if success and write req allocate response buffer */
if (err == ATT_SUCCESS && opcode == ATT_PDU_WRITE_REQ)
{
if ((pBuf = attMsgAlloc(L2C_PAYLOAD_START + ATT_WRITE_RSP_LEN)) != NULL)
{
/* build and send PDU */
p = pBuf + L2C_PAYLOAD_START;
UINT8_TO_BSTREAM(p, ATT_PDU_WRITE_RSP);
L2cDataReq(L2C_CID_ATT, pCcb->handle, ATT_WRITE_RSP_LEN, pBuf);
}
}
}
}
/* else attribute not found */
else
{
err = ATT_ERR_HANDLE;
}
/* send error response for write req only */
if (err && (opcode == ATT_PDU_WRITE_REQ))
{
attsErrRsp(pCcb->handle, ATT_PDU_WRITE_REQ, handle, err);
}
}
