static void l2cMasterRxSignalingPkt(uint16_t handle, uint16_t l2cLen, uint8_t *pPacket)
{
uint8_t code;
uint8_t id;
uint16_t len;
hciConnSpec_t connSpec;
/* parse code, len, and identifier */
pPacket += L2C_PAYLOAD_START;
BSTREAM_TO_UINT8(code, pPacket);
BSTREAM_TO_UINT8(id, pPacket);
BSTREAM_TO_UINT16(len, pPacket);
/* verify signaling length vs. l2c length
* verify this is a conn param update rsp
* verify parameter length
*/
if ((l2cLen != (len + L2C_SIG_HDR_LEN)) ||
(code != L2C_SIG_CONN_UPDATE_REQ) ||
(len != L2C_SIG_CONN_UPDATE_REQ_LEN))
{
L2C_TRACE_WARN3("invalid msg code:%d len:%d l2cLen:%d", code, len, l2cLen);
/* reject all unknown or invalid commands except command reject. */
if (code != L2C_SIG_CMD_REJ)
{
l2cSendCmdReject(handle, id, L2C_REJ_NOT_UNDERSTOOD);
}
return;
}
/* parse parameters */
BSTREAM_TO_UINT16(connSpec.connIntervalMin, pPacket);
BSTREAM_TO_UINT16(connSpec.connIntervalMax, pPacket);
BSTREAM_TO_UINT16(connSpec.connLatency, pPacket);
BSTREAM_TO_UINT16(connSpec.supTimeout, pPacket);
connSpec.minCeLen = 0;
connSpec.maxCeLen = 0;
/* check parameter range */
if ((connSpec.connIntervalMin < HCI_CONN_INTERVAL_MIN) ||
(connSpec.connIntervalMin > HCI_CONN_INTERVAL_MAX) ||
(connSpec.connIntervalMin > connSpec.connIntervalMax) ||
(connSpec.connIntervalMax < HCI_CONN_INTERVAL_MIN) ||
(connSpec.connIntervalMax > HCI_CONN_INTERVAL_MAX) ||
(connSpec.connLatency > HCI_CONN_LATENCY_MAX) ||
(connSpec.supTimeout < HCI_SUP_TIMEOUT_MIN) ||
(connSpec.supTimeout > HCI_SUP_TIMEOUT_MAX))
{
L2cDmConnUpdateRsp(id, handle, L2C_CONN_PARAM_REJECTED);
return;
}
DmL2cConnUpdateInd(id, handle, &connSpec);
}
void attcProcErrRsp(attcCcb_t *pCcb, uint16_t len, uint8_t *pPacket, attEvt_t *pEvt)
{
uint8_t *p;
p = pPacket + L2C_PAYLOAD_START + ATT_HDR_LEN;
/* set callback event from stored method */
pEvt->hdr.event = pCcb->outReq.hdr.event;
/* ignore request opcode in the error response */
p++;
/* if request was a read or write with a specific handle */
if (pEvt->hdr.event == ATTC_READ_RSP || pEvt->hdr.event == ATTC_READ_LONG_RSP ||
pEvt->hdr.event == ATTC_WRITE_RSP || pEvt->hdr.event == ATTC_PREPARE_WRITE_RSP)
{
/* ignore handle in the error response; callback will use stored handle from request */
p += 2;
}
else
{
/* set handle from packet */
BSTREAM_TO_UINT16(pEvt->handle, p);
}
/* set status from error code in packet, but verify it's not 'success' */
BSTREAM_TO_UINT8(pEvt->hdr.status, p);
if (pEvt->hdr.status == ATT_SUCCESS)
{
pEvt->hdr.status = ATT_ERR_UNDEFINED;
}
/* no parameters so clear length */
pEvt->valueLen = 0;
}
void attcProcInd(attcCcb_t *pCcb, uint16_t len, uint8_t *pPacket)
{
attEvt_t evt;
uint8_t *p;
uint8_t *pPkt;
p = pPacket + L2C_PAYLOAD_START;
/* parse packet and set callback event struct */
evt.hdr.event = ATT_OPCODE_2_METHOD(*p++);
BSTREAM_TO_UINT16(evt.handle, p);
evt.pValue = p;
evt.valueLen = len - ATT_HDR_LEN - sizeof(uint16_t);
evt.hdr.param = pCcb->pMainCcb->connId;
evt.hdr.status = ATT_SUCCESS;
evt.continuing = FALSE;
/* verify handle and call callback */
if (evt.handle != 0)
{
(*attCb.cback)(&evt);
}
/* if indication send confirm */
if (evt.hdr.event == ATT_METHOD_VALUE_IND)
{
if (!pCcb->flowDisabled)
{
if ((pPkt = attMsgAlloc(ATT_VALUE_CNF_LEN + L2C_PAYLOAD_START)) != NULL)
{
*(pPkt + L2C_PAYLOAD_START) = ATT_PDU_VALUE_CNF;
L2cDataReq(L2C_CID_ATT, pCcb->pMainCcb->handle, ATT_VALUE_CNF_LEN, pPkt);
}
}
else
{
/* mark confirm as pending; will be sent when flow enabled */
pCcb->cnfPending = TRUE;
}
}
}
uint8_t BlpcBpsValueUpdate(uint16_t *pHdlList, attEvt_t *pMsg)
{
uint16_t feature;
uint8_t *p;
uint8_t status = ATT_SUCCESS;
/* blood pressure measurement */
if (pMsg->handle == pHdlList[BLPC_BPS_BPM_HDL_IDX])
{
APP_TRACE_INFO0("BP measurement");
/* parse value */
blpcBpsParseBpm(pMsg->pValue, pMsg->valueLen);
}
/* intermediate cuff pressure */
else if (pMsg->handle == pHdlList[BLPC_BPS_ICP_HDL_IDX])
{
APP_TRACE_INFO0("BP intermed. cuff pressure");
/* parse value */
blpcBpsParseBpm(pMsg->pValue, pMsg->valueLen);
}
/* blood pressure feature */
else if (pMsg->handle == pHdlList[BLPC_BPS_BPF_HDL_IDX])
{
/* parse value */
p = pMsg->pValue;
BSTREAM_TO_UINT16(feature, p);
APP_TRACE_INFO1("BP feature:0x%04x", feature);
}
else
{
status = ATT_ERR_NOT_FOUND;
}
return status;
}
void attcProcFindOrReadRsp(attcCcb_t *pCcb, uint16_t len, uint8_t *pPacket, attEvt_t *pEvt)
{
uint8_t *p;
uint8_t *pEnd;
uint16_t handle;
uint16_t nextHandle;
uint16_t prevHandle;
uint8_t paramLen;
p = pPacket + L2C_PAYLOAD_START + ATT_HDR_LEN;
pEnd = pPacket + L2C_PAYLOAD_START + len;
/* parameter length depends on packet type */
if (pCcb->outReq.hdr.event == ATTC_MSG_API_FIND_INFO)
{
/* length in find info response is coded by UUID */
if (*p++ == ATT_FIND_HANDLE_16_UUID)
{
paramLen = ATT_16_UUID_LEN;
}
else
{
paramLen = ATT_128_UUID_LEN;
}
}
else if (pCcb->outReq.hdr.event == ATTC_MSG_API_READ_BY_TYPE)
{
/* length in read by type response is handle plus parameter length */
paramLen = *p++ - sizeof(uint16_t);
}
else
{
/* length in read by group type response is two handles plus parameter length */
paramLen = *p++ - (2 * sizeof(uint16_t));
}
/* get and verify all handles */
nextHandle = pCcb->outReqParams.h.startHandle;
while (p < pEnd)
{
/* get and compare handle */
BSTREAM_TO_UINT16(handle, p);
if (handle == 0 || nextHandle == 0 || handle < nextHandle ||
handle > pCcb->outReqParams.h.endHandle)
{
pEvt->hdr.status = ATT_ERR_INVALID_RSP;
break;
}
/* if read by group type response get second handle */
if (pCcb->outReq.hdr.event == ATTC_MSG_API_READ_BY_GROUP_TYPE)
{
prevHandle = handle;
BSTREAM_TO_UINT16(handle, p);
if (handle == 0 || handle < prevHandle || handle < nextHandle ||
handle > pCcb->outReqParams.h.endHandle)
{
pEvt->hdr.status = ATT_ERR_INVALID_RSP;
break;
}
}
/* set next expected handle, with special case for max handle */
if (handle == ATT_HANDLE_MAX)
{
nextHandle = 0;
}
else
{
nextHandle = handle + 1;
}
/* skip over parameter */
p += paramLen;
/* check for truncated response */
if (p > pEnd)
{
pEvt->hdr.status = ATT_ERR_INVALID_RSP;
break;
}
}
/* if response was correct */
if (pEvt->hdr.status == ATT_SUCCESS)
{
/* if continuing */
if (pCcb->outReq.hdr.status == ATTC_CONTINUING)
{
/* if all handles read */
if (nextHandle == 0 || nextHandle == (pCcb->outReqParams.h.endHandle + 1))
{
/* we're done */
pCcb->outReq.hdr.status = ATTC_NOT_CONTINUING;
}
/* else set up for next request */
else
{
pCcb->outReqParams.h.startHandle = nextHandle;
pCcb->outReq.handle = nextHandle;
}
}
}
}
bool_t smpProcRcvKey(smpCcb_t *pCcb, dmSecKeyIndEvt_t *pKeyInd, uint8_t *pBuf, uint8_t keyDist)
{
bool_t keyIndReady = FALSE;
bool_t done = FALSE;
uint8_t cmdCode;
/* go to start of packet */
pBuf += L2C_PAYLOAD_START;
cmdCode = *pBuf++;
if (cmdCode == SMP_CMD_ENC_INFO)
{
/* parse encryption information packet */
Calc128Cpy(pKeyInd->keyData.ltk.key, pBuf);
}
else if (cmdCode == SMP_CMD_MASTER_ID)
{
/* parse master identification packet */
BSTREAM_TO_UINT16(pKeyInd->keyData.ltk.ediv, pBuf);
memcpy(pKeyInd->keyData.ltk.rand, pBuf, SMP_RAND8_LEN);
pKeyInd->secLevel = (pCcb->auth & SMP_AUTH_MITM_FLAG) ? DM_SEC_LEVEL_ENC_AUTH : DM_SEC_LEVEL_ENC;
pKeyInd->type = DM_KEY_PEER_LTK;
keyIndReady = TRUE;
}
else if (cmdCode == SMP_CMD_ID_INFO)
{
/* parse identity information packet */
Calc128Cpy(pKeyInd->keyData.irk.key, pBuf);
}
else if (cmdCode == SMP_CMD_ID_ADDR_INFO)
{
/* parse identity address information packet */
BSTREAM_TO_UINT8(pKeyInd->keyData.irk.addrType, pBuf);
BSTREAM_TO_BDA(pKeyInd->keyData.irk.bdAddr, pBuf);
pKeyInd->type = DM_KEY_IRK;
keyIndReady = TRUE;
}
else if (cmdCode == SMP_CMD_SIGN_INFO)
{
/* parse signing information packet */
Calc128Cpy(pKeyInd->keyData.csrk.key, pBuf);
pKeyInd->type = DM_KEY_CSRK;
keyIndReady = TRUE;
}
/* set up to receive next key */
/* if just got first part of LTK or IRK */
if (pCcb->nextCmdCode == SMP_CMD_ENC_INFO || pCcb->nextCmdCode == SMP_CMD_ID_INFO)
{
/* wait for second part of LTK or IRK info */
pCcb->nextCmdCode++;
}
/* else if got LTK and need IRK */
else if ((keyDist & SMP_KEY_DIST_ID) && (pCcb->nextCmdCode == SMP_CMD_MASTER_ID))
{
/* wait for first part of IRK */
pCcb->nextCmdCode = SMP_CMD_ID_INFO;
}
/* else if got LTK or IRK and need SRK */
else if ((keyDist & SMP_KEY_DIST_SIGN) &&
(pCcb->nextCmdCode == SMP_CMD_MASTER_ID || pCcb->nextCmdCode == SMP_CMD_ID_ADDR_INFO))
{
/* wait for SRK */
pCcb->nextCmdCode = SMP_CMD_SIGN_INFO;
}
else
{
/* done receiving keys */
done = TRUE;
}
/* call callback if key ready */
if (keyIndReady)
{
pKeyInd->hdr.event = DM_SEC_KEY_IND;
DmSmpCbackExec((dmEvt_t *) pKeyInd);
}
return done;
}
void blpcBpsParseBpm(uint8_t *pValue, uint16_t len)
{
uint8_t flags;
uint16_t systolic, diastolic, map;
uint16_t year;
uint8_t month, day, hour, min, sec;
uint16_t pulseRate;
uint8_t userId;
uint16_t measStatus;
uint16_t minLen = CH_BPM_FLAGS_LEN + CH_BPM_MEAS_LEN;
if (len > 0)
{
/* get flags */
BSTREAM_TO_UINT8(flags, pValue);
/* determine expected minimum length based on flags */
if (flags & CH_BPM_FLAG_TIMESTAMP)
{
minLen += CH_BPM_TIMESTAMP_LEN;
}
if (flags & CH_BPM_FLAG_PULSE_RATE)
{
minLen += CH_BPM_PULSE_RATE_LEN;
}
if (flags & CH_BPM_FLAG_USER_ID)
{
minLen += CH_BPM_USER_ID_LEN;
}
if (flags & CH_BPM_FLAG_MEAS_STATUS)
{
minLen += CH_BPM_MEAS_STATUS_LEN;
}
}
/* verify length */
if (len < minLen)
{
APP_TRACE_INFO2("Blood Pressure meas len:%d minLen:%d", len, minLen);
return;
}
/* blood pressure */
BSTREAM_TO_UINT16(systolic, pValue);
BSTREAM_TO_UINT16(diastolic, pValue);
BSTREAM_TO_UINT16(map, pValue);
APP_TRACE_INFO3(" Systolic:0x%04x Diastolic:0x%04x MAP:0x%04x",
systolic, diastolic, map);
/* timestamp */
if (flags & CH_BPM_FLAG_TIMESTAMP)
{
BSTREAM_TO_UINT16(year, pValue);
BSTREAM_TO_UINT8(month, pValue);
BSTREAM_TO_UINT8(day, pValue);
BSTREAM_TO_UINT8(hour, pValue);
BSTREAM_TO_UINT8(min, pValue);
BSTREAM_TO_UINT8(sec, pValue);
APP_TRACE_INFO3(" Date: %d/%d/%d", month, day, year);
APP_TRACE_INFO3(" Time: %02d:%02d:%02d", hour, min, sec);
}
/* pulse rate */
if (flags & CH_BPM_FLAG_PULSE_RATE)
{
BSTREAM_TO_UINT16(pulseRate, pValue);
APP_TRACE_INFO1(" Pulse rate:0x%04x", pulseRate);
}
/* user id */
if (flags & CH_BPM_FLAG_USER_ID)
{
BSTREAM_TO_UINT8(userId, pValue);
APP_TRACE_INFO1(" User ID:%d", userId);
}
/* measurement status */
if (flags & CH_BPM_FLAG_MEAS_STATUS)
{
BSTREAM_TO_UINT16(measStatus, pValue);
APP_TRACE_INFO1(" Meas. status:0x%04x", measStatus);
}
APP_TRACE_INFO1(" Flags:0x%02x", flags);
}
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);
}
}
