void smprActCnfVerify(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
/* compare calculated confirm value with value received earlier */
if (memcmp(pMsg->aes.pCiphertext, pCcb->pScr->buf.b3, SMP_CONFIRM_LEN) != 0)
{
/* confirm values don't match; update repeated attempts count */
pCcb->attempts++;
SmpDbPairingFailed(pCcb->connId);
pMsg->hdr.status = SMP_ERR_CONFIRM_VALUE;
if (pCcb->attempts == pSmpCfg->maxAttempts)
{
/* max attempts reached */
pMsg->hdr.event = SMP_MSG_INT_MAX_ATTEMPTS;
}
else
{
/* else just fail */
pMsg->hdr.event = SMP_MSG_API_CANCEL_REQ;
}
smpSmExecute(pCcb, pMsg);
return;
}
/* do STK calculation: key, responder rand, initiator rand */
smpCalcS1(pCcb, pCcb->pScr->buf.b1, pCcb->pScr->buf.b4, pCcb->pScr->buf.b2);
}
void smpAuthReq(smpCcb_t *pCcb, uint8_t oob, uint8_t display)
{
/* use a union to save a bit of memory on the stack */
union
{
smpDmAuthRsp_t authRsp;
dmSecAuthReqIndEvt_t authReq;
} buf;
/* if authenticated pairing */
if (pCcb->auth & SMP_AUTH_MITM_FLAG)
{
/* request pin or oob from user */
buf.authReq.hdr.param = pCcb->connId;
buf.authReq.hdr.event = DM_SEC_AUTH_REQ_IND;
buf.authReq.oob = oob;
buf.authReq.display = display;
DmSmpCbackExec((dmEvt_t *) &buf.authReq);
}
else
{
/* else use just works; send ourselves a auth rsp with all zero pin */
buf.authRsp.hdr.param = pCcb->connId;
buf.authRsp.hdr.event = SMP_MSG_API_AUTH_RSP;
buf.authRsp.authData[0] = 0;
buf.authRsp.authData[1] = 0;
buf.authRsp.authData[2] = 0;
buf.authRsp.authDataLen = SMP_PIN_LEN;
smpSmExecute(pCcb, (smpMsg_t *) &buf.authRsp);
}
}
void SmpHandler(wsfEventMask_t event, wsfMsgHdr_t *pMsg)
{
smpCcb_t *pCcb;
/* Handle message */
if (pMsg != NULL)
{
if (pMsg->event == SMP_DB_SERVICE_IND)
{
SmpDbService();
}
else
{
if (pMsg->event == SMP_MSG_WSF_CMAC_CMPL)
{
secCmacMsg_t *pCmac = (secCmacMsg_t *) pMsg;
/* Free the plain text buffer that was allocated and passed into SecCmac */
if (pCmac->pPlainText)
{
WsfBufFree(pCmac->pPlainText);
}
}
/* get connection control block */
pCcb = smpCcbByConnId((dmConnId_t) pMsg->param);
/* verify connection is open */
if (pCcb->connId != DM_CONN_ID_NONE)
{
/* if AES result verify it is not stale */
if (pMsg->event == SMP_MSG_WSF_AES_CMPL && pCcb->token != pMsg->status)
{
SMP_TRACE_WARN2("AES token mismatch: %d %d", pCcb->token, pMsg->status);
}
else
{
/* send to state machine */
smpSmExecute(pCcb, (smpMsg_t *) pMsg);
}
}
}
}
/* Handle events */
else if (event)
{
}
}
void smprActRcvKey(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
uint8_t keyDist;
/* get initiator key distribution */
keyDist = pCcb->pairReq[SMP_IKEYDIST_POS] & pCcb->pairRsp[SMP_IKEYDIST_POS];
/* process received key */
if (smpProcRcvKey(pCcb, &pCcb->pScr->keyInd, pMsg->data.pPacket, keyDist))
{
/* no more keys to receive; send ourselves pairing complete msg */
pMsg->hdr.event = SMP_MSG_INT_PAIRING_CMPL;
smpSmExecute(pCcb, pMsg);
}
}
void smprActSendKey(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
uint8_t keyDist;
/* get responder key distribution */
keyDist = pCcb->pairReq[SMP_RKEYDIST_POS] & pCcb->pairRsp[SMP_RKEYDIST_POS];
/* send next key; if done sending keys set up to receive keys */
if ((pCcb->nextCmdCode == 0) && smpSendKey(pCcb, keyDist))
{
pCcb->nextCmdCode = 0;
/* get initiator key distribution */
keyDist = pCcb->pairReq[SMP_IKEYDIST_POS] & pCcb->pairRsp[SMP_IKEYDIST_POS];
/* set up to receive first key distribution packet */
if (keyDist & SMP_KEY_DIST_ENC)
{
if (smpCb.lescSupported && pCcb->pScCcb->lescEnabled)
{
if (keyDist & SMP_KEY_DIST_ID)
{
pCcb->nextCmdCode = SMP_CMD_ID_INFO;
}
}
else
{
pCcb->nextCmdCode = SMP_CMD_ENC_INFO;
}
}
else if (keyDist & SMP_KEY_DIST_ID)
{
pCcb->nextCmdCode = SMP_CMD_ID_INFO;
}
else if (keyDist & SMP_KEY_DIST_SIGN)
{
pCcb->nextCmdCode = SMP_CMD_SIGN_INFO;
}
if (pCcb->nextCmdCode == 0)
{
/* no keys to receive; send ourselves pairing complete msg */
pMsg->hdr.event = SMP_MSG_INT_PAIRING_CMPL;
smpSmExecute(pCcb, pMsg);
}
}
}
void smprActProcPairReq(smpCcb_t *pCcb, smpMsg_t *pMsg)
{
dmSecPairIndEvt_t pairInd;
uint8_t *p;
/* allocate scratch buffer */
if (pCcb->pScr == NULL)
{
if ((pCcb->pScr = WsfBufAlloc(sizeof(smpScratch_t))) == NULL)
{
/* alloc failed; cancel pairing */
pMsg->hdr.status = SMP_ERR_UNSPECIFIED;
pMsg->hdr.event = SMP_MSG_API_CANCEL_REQ;
smpSmExecute(pCcb, pMsg);
return;
}
}
else
{
/* should not happen */
SMP_TRACE_ERR0("pScr already allocated");
}
/* set connection busy */
DmConnSetIdle(pCcb->connId, DM_IDLE_SMP_PAIR, DM_CONN_BUSY);
p = pMsg->data.pPacket + L2C_PAYLOAD_START;
/* store packet for later */
memcpy(pCcb->pairReq, p, SMP_PAIR_REQ_LEN);
/* parse packet to callback event structure */
p++; /* skip command code */
p++; /* skip IO capabilities */
BSTREAM_TO_UINT8(pairInd.oob, p);
BSTREAM_TO_UINT8(pairInd.auth, p);
p++; /* skip max key len */
BSTREAM_TO_UINT8(pairInd.iKeyDist, p);
BSTREAM_TO_UINT8(pairInd.rKeyDist, p);
/* call app callback */
pairInd.hdr.param = pCcb->connId;
pairInd.hdr.event = DM_SEC_PAIR_IND;
DmSmpCbackExec((dmEvt_t *) &pairInd);
}
static void smpL2cDataCback(uint16_t handle, uint16_t len, uint8_t *pPacket)
{
uint8_t cmdCode;
smpCcb_t *pCcb;
/* get connection control block for this handle, ignore packet if not found */
if ((pCcb = smpCcbByHandle(handle)) == NULL)
{
return;
}
/* parse command code */
cmdCode = *(pPacket + L2C_PAYLOAD_START);
/* verify length and that command is the expected command or pairing failed */
if ((cmdCode >= SMP_CMD_PAIR_REQ && cmdCode < SMP_CMD_MAX) &&
(len == smpPktLenTbl[cmdCode]) &&
((cmdCode == pCcb->nextCmdCode) || (cmdCode == SMP_CMD_PAIR_FAIL)))
{
smpMsg_t msg;
/* send to state machine */
if (cmdCode == SMP_CMD_PAIR_FAIL)
{
msg.hdr.event = SMP_MSG_CMD_PAIRING_FAILED;
msg.hdr.status = *(pPacket + L2C_PAYLOAD_START + SMP_HDR_LEN);
}
else
{
msg.hdr.event = SMP_MSG_CMD_PKT;
}
msg.hdr.param = pCcb->connId;
msg.data.pPacket = pPacket;
smpSmExecute(pCcb, &msg);
}
/* else ignore it */
else
{
SMP_TRACE_WARN3("unexpected packet cmd:%d len:%d, expected:%d", cmdCode, len, pCcb->nextCmdCode);
}
}
void smpCalcS1(smpCcb_t *pCcb, uint8_t *pKey, uint8_t *pRand1, uint8_t *pRand2)
{
uint8_t buf[HCI_ENCRYPT_DATA_LEN];
/* note all numbers contained in byte arrays are little endian */
/* construct parameter r' from r1 and r2 */
Calc128Cpy64(buf, pRand2);
Calc128Cpy64(&buf[SMP_RAND8_LEN], pRand1);
/* encrypt */
pCcb->token = SecAes(pKey, buf, smpCb.handlerId, pCcb->connId, SMP_MSG_WSF_AES_CMPL);
if (pCcb->token == SEC_TOKEN_INVALID)
{
wsfMsgHdr_t hdr;
/* fail on invalid token */
hdr.status = SMP_ERR_UNSPECIFIED;
hdr.event = SMP_MSG_API_CANCEL_REQ;
smpSmExecute(pCcb, (smpMsg_t *) &hdr);
}
}
static void smpL2cCtrlCback(wsfMsgHdr_t *pMsg)
{
smpCcb_t *pCcb;
uint8_t *pPkt;
/* get connection control block */
pCcb = smpCcbByConnId((dmConnId_t) pMsg->param);
/* verify connection is open */
if (pCcb->connId != DM_CONN_ID_NONE)
{
/* set flow */
pCcb->flowDisabled = (pMsg->event == L2C_CTRL_FLOW_DISABLE_IND);
/* if data flow enabled */
if (!pCcb->flowDisabled)
{
/* if packet in qeueue */
if (pCcb->pQueued != NULL)
{
/* send queued packet */
pPkt = pCcb->pQueued;
pCcb->pQueued = NULL;
smpSendPkt(pCcb, pPkt);
}
/* if SMP state not idle */
if (!smpStateIdle(pCcb))
{
/* trigger send of next key */
pMsg->event = SMP_MSG_INT_SEND_NEXT_KEY;
smpSmExecute(pCcb, (smpMsg_t *) pMsg);
}
}
}
}
bool_t smpProcPairing(smpCcb_t *pCcb, uint8_t *pOob, uint8_t *pDisplay)
{
bool_t justWorks = TRUE;
uint8_t localAuth;
wsfMsgHdr_t hdr;
*pDisplay = FALSE;
*pOob = FALSE;
/* if OOB available use that */
if (pCcb->pairReq[SMP_OOB_POS] == SMP_OOB_DATA_PRESENT &&
pCcb->pairRsp[SMP_OOB_POS] == SMP_OOB_DATA_PRESENT)
{
*pOob = SMP_OOB_DATA_PRESENT;
justWorks = FALSE;
}
/* if either device set mitm flag */
else if ((pCcb->pairReq[SMP_AUTHREQ_POS] & SMP_AUTH_MITM_FLAG) ||
(pCcb->pairRsp[SMP_AUTHREQ_POS] & SMP_AUTH_MITM_FLAG))
{
/* check for compatible I/O settings */
if ((pCcb->pairReq[SMP_IO_POS] != SMP_IO_NO_IN_NO_OUT) && /* initiator has i/o and */
(pCcb->pairRsp[SMP_IO_POS] != SMP_IO_NO_IN_NO_OUT) && /* responder has i/o and */
!(((pCcb->pairReq[SMP_IO_POS] == SMP_IO_DISP_ONLY) || /* both don't have display only */
(pCcb->pairReq[SMP_IO_POS] == SMP_IO_DISP_YES_NO)) &&
((pCcb->pairRsp[SMP_IO_POS] == SMP_IO_DISP_ONLY) ||
(pCcb->pairRsp[SMP_IO_POS] == SMP_IO_DISP_YES_NO))))
{
/* use pin */
justWorks = FALSE;
/* check if pin should be displayed (as initiator) */
*pDisplay =
((pCcb->pairReq[SMP_IO_POS] == SMP_IO_DISP_ONLY) || /* initiator is display only or */
(pCcb->pairReq[SMP_IO_POS] == SMP_IO_DISP_YES_NO) || /* initiator is display y/n or */
((pCcb->pairReq[SMP_IO_POS] == SMP_IO_KEY_DISP) && /* initiator is key/display and */
(pCcb->pairRsp[SMP_IO_POS] == SMP_IO_KEY_ONLY || /* responder is key only or key/display */
pCcb->pairRsp[SMP_IO_POS] == SMP_IO_KEY_DISP)));
/* invert display setting if we are not initiator and both are not key only */
if (!(pCcb->pairRsp[SMP_IO_POS] == SMP_IO_KEY_ONLY &&
pCcb->pairReq[SMP_IO_POS] == SMP_IO_KEY_ONLY))
{
*pDisplay ^= !pCcb->initiator;
}
}
}
if (!justWorks)
{
/* set auth flags with mitm bit set */
pCcb->auth = (pCcb->pairReq[SMP_AUTHREQ_POS] & pCcb->pairRsp[SMP_AUTHREQ_POS]) | SMP_AUTH_MITM_FLAG;
}
else
{
/* set auth flags with mitm bit cleared */
pCcb->auth = pCcb->pairReq[SMP_AUTHREQ_POS] & pCcb->pairRsp[SMP_AUTHREQ_POS] & ~SMP_AUTH_MITM_FLAG;
}
/* if we ended up with 'just works' but the device configuration requires authentication */
localAuth = (pCcb->initiator) ? pCcb->pairReq[SMP_AUTHREQ_POS] : pCcb->pairRsp[SMP_AUTHREQ_POS];
if (justWorks && (pSmpCfg->auth & localAuth & SMP_AUTH_MITM_FLAG))
{
/* cancel pairing */
hdr.param = pCcb->connId;
hdr.status = SMP_ERR_AUTH_REQ;
hdr.event = SMP_MSG_API_CANCEL_REQ;
smpSmExecute(pCcb, (smpMsg_t *) &hdr);
return FALSE;
}
/* if max encryption key is less than our minimum */
if (pCcb->pairReq[SMP_MAXKEY_POS] < pSmpCfg->minKeyLen ||
pCcb->pairRsp[SMP_MAXKEY_POS] < pSmpCfg->minKeyLen)
{
/* cancel pairing */
hdr.param = pCcb->connId;
hdr.status = SMP_ERR_ENC_KEY_SIZE;
hdr.event = SMP_MSG_API_CANCEL_REQ;
smpSmExecute(pCcb, (smpMsg_t *) &hdr);
return FALSE;
}
return TRUE;
}
void smpCalcC1Part2(smpCcb_t *pCcb, uint8_t *pKey, uint8_t *pPart1)
{
uint8_t buf[HCI_ENCRYPT_DATA_LEN];
uint8_t *p;
uint8_t i;
uint8_t *pIaddr;
uint8_t *pRaddr;
/* set initiator/responder addresss */
if (pCcb->initiator)
{
/* use local device's RPA */
pIaddr = DmConnLocalRpa(pCcb->connId);
/* if local device's not using RPA */
if (BdaIsZeros(pIaddr))
{
/* use local device's address */
pIaddr = DmConnLocalAddr(pCcb->connId);
}
/* use peer device's RPA */
pRaddr = DmConnPeerRpa(pCcb->connId);
/* if peer device's not using RPA */
if (BdaIsZeros(pRaddr))
{
/* use peer device's address */
pRaddr = DmConnPeerAddr(pCcb->connId);
}
}
else
{
/* use peer device's RPA */
pIaddr = DmConnPeerRpa(pCcb->connId);
/* if peer device's not using RPA */
if (BdaIsZeros(pIaddr))
{
/* use peer device's address */
pIaddr = DmConnPeerAddr(pCcb->connId);
}
/* use local device's RPA */
pRaddr = DmConnLocalRpa(pCcb->connId);
/* if local device's not using RPA */
if (BdaIsZeros(pRaddr))
{
/* use local device's address */
pRaddr = DmConnLocalAddr(pCcb->connId);
}
}
/* note all numbers contained in byte arrays are little endian */
/* create parameter from xor of part 1 result with ia, ra, and pad */
p = buf;
for (i = BDA_ADDR_LEN; i > 0; i--)
{
*p++ = *pRaddr++ ^ *pPart1++;
}
for (i = BDA_ADDR_LEN; i > 0; i--)
{
*p++ = *pIaddr++ ^ *pPart1++;
}
*p++ = *pPart1++;
*p++ = *pPart1++;
*p++ = *pPart1++;
*p++ = *pPart1++;
/* encrypt */
pCcb->token = SecAes(pKey, buf, smpCb.handlerId, pCcb->connId, SMP_MSG_WSF_AES_CMPL);
if (pCcb->token == SEC_TOKEN_INVALID)
{
wsfMsgHdr_t hdr;
/* fail on invalid token */
hdr.status = SMP_ERR_UNSPECIFIED;
hdr.event = SMP_MSG_API_CANCEL_REQ;
smpSmExecute(pCcb, (smpMsg_t *) &hdr);
}
}
void smpCalcC1Part1(smpCcb_t *pCcb, uint8_t *pKey, uint8_t *pRand)
{
uint8_t buf[HCI_ENCRYPT_DATA_LEN];
uint8_t *p;
uint8_t i;
uint8_t iAddrType;
uint8_t rAddrType;
/* set initiator/responder address types */
if (pCcb->initiator)
{
/* if local device's using RPA */
if (!BdaIsZeros(DmConnLocalRpa(pCcb->connId)))
{
iAddrType = DM_ADDR_RANDOM;
}
else
{
iAddrType = DmConnLocalAddrType(pCcb->connId);
}
/* if peer device's using RPA */
if (!BdaIsZeros(DmConnPeerRpa(pCcb->connId)))
{
rAddrType = DM_ADDR_RANDOM;
}
else
{
rAddrType = DmConnPeerAddrType(pCcb->connId);
}
}
else
{
/* if peer device's using RPA */
if (!BdaIsZeros(DmConnPeerRpa(pCcb->connId)))
{
iAddrType = DM_ADDR_RANDOM;
}
else
{
iAddrType = DmConnPeerAddrType(pCcb->connId);
}
/* if local device's using RPA */
if (!BdaIsZeros(DmConnLocalRpa(pCcb->connId)))
{
rAddrType = DM_ADDR_RANDOM;
}
else
{
rAddrType = DmConnLocalAddrType(pCcb->connId);
}
}
/* note all numbers contained in byte arrays are little endian */
/* create parameter from xor of r and pres, preq, rat, and iat */
p = buf;
*p++ = iAddrType ^ *pRand++;
*p++ = rAddrType ^ *pRand++;
for (i = 0; i < SMP_PAIR_REQ_LEN; i++)
{
*p++ = pCcb->pairReq[i] ^ *pRand++;
}
for (i = 0; i < SMP_PAIR_RSP_LEN; i++)
{
*p++ = pCcb->pairRsp[i] ^ *pRand++;
}
/* encrypt */
pCcb->token = SecAes(pKey, buf, smpCb.handlerId, pCcb->connId, SMP_MSG_WSF_AES_CMPL);
if (pCcb->token == SEC_TOKEN_INVALID)
{
wsfMsgHdr_t hdr;
/* fail on invalid token */
hdr.status = SMP_ERR_UNSPECIFIED;
hdr.event = SMP_MSG_API_CANCEL_REQ;
smpSmExecute(pCcb, (smpMsg_t *) &hdr);
}
}
static void smpDmConnCback(dmEvt_t *pDmEvt)
{
smpCcb_t *pCcb;
wsfMsgHdr_t hdr;
pCcb = smpCcbByConnId((dmConnId_t) pDmEvt->hdr.param);
/* if new connection created */
if (pDmEvt->hdr.event == DM_CONN_OPEN_IND)
{
/* set up state machine for master or slave */
if (DmConnRole((dmConnId_t) pDmEvt->hdr.param) == DM_ROLE_MASTER)
{
pCcb->initiator = TRUE;
pCcb->nextCmdCode = SMP_CMD_SECURITY_REQ;
}
else
{
pCcb->initiator = FALSE;
pCcb->nextCmdCode = SMP_CMD_PAIR_REQ;
}
/* initialize control block */
pCcb->handle = pDmEvt->connOpen.handle;
pCcb->connId = (dmConnId_t) pDmEvt->hdr.param;
pCcb->secReq = FALSE;
pCcb->flowDisabled = FALSE;
pCcb->attempts = SmpDbGetFailureCount((dmConnId_t) pDmEvt->hdr.param);
pCcb->lastSentKey = 0;
pCcb->state = 0;
/* Resume the attempts state if necessary */
smpResumeAttemptsState((dmConnId_t) pDmEvt->hdr.param);
}
/* else if connection has been opened */
else if (pCcb->connId != DM_CONN_ID_NONE)
{
/* handle close */
if (pDmEvt->hdr.event == DM_CONN_CLOSE_IND)
{
/* store attempts count */
SmpDbSetFailureCount((dmConnId_t) pDmEvt->hdr.param, pCcb->attempts);
/* send to state machine */
hdr.param = pDmEvt->hdr.param;
hdr.event = SMP_MSG_DM_CONN_CLOSE;
hdr.status = pDmEvt->connClose.reason + DM_SEC_HCI_ERR_BASE;
smpSmExecute(pCcb, (smpMsg_t *) &hdr);
/* clear conn ID after handling event */
pCcb->connId = DM_CONN_ID_NONE;
/* free queued packet buffer */
if (pCcb->pQueued != NULL)
{
WsfMsgFree(pCcb->pQueued);
pCcb->pQueued = NULL;
}
}
}
}
