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 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); }
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 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); } }