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);
}
}
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;
}
}
}
void smpSendPkt(smpCcb_t *pCcb, uint8_t *pPkt)
{
/* if flow disabled */
if (pCcb->flowDisabled)
{
/* if packet already queued discard it and replace it with this new packet */
if (pCcb->pQueued != NULL)
{
SMP_TRACE_WARN1("smpSendPkt packet discarded cmd:%d", pCcb->pQueued[L2C_PAYLOAD_START]);
WsfMsgFree(pCcb->pQueued);
}
/* queue packet */
pCcb->pQueued = pPkt;
}
/* else send it to L2CAP */
else
{
L2cDataReq(L2C_CID_SMP, pCcb->handle, smpPktLenTbl[pPkt[L2C_PAYLOAD_START]], pPkt);
}
}
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);
}
}
