void initRunMode() {
#if ENABLE_SLOTS
setupSlots();
#endif
mcu_clearTimer();
//TODO: Figure out that dest = destorig shit
dest = destorig;
#if !(ENABLE_SLOTS)
queryReplyCRC = crc16_ccitt(&queryReply[0],2);
queryReply[3] = (unsigned char) queryReplyCRC;
queryReply[2] = (unsigned char) mcu_swapBytes(queryReplyCRC);
#endif
#if SENSOR_DATA_IN_ID
// this branch is for sensor data in the id
ackReply[2] = SENSOR_DATA_TYPE_ID;
state = STATE_READ_SENSOR;
timeToSample++;
#else
ackReplyCRC = crc16_ccitt(&ackReply[0], 14);
ackReply[15] = (unsigned char) ackReplyCRC;
ackReply[14] = (unsigned char) mcu_swapBytes(ackReplyCRC);
#endif
#if ENABLE_SESSIONS
initialize_sessions();
#endif
state = STATE_READY;
}
uint16_t XN297_UnscramblePayload(uint8_t *data, int len, const uint8_t *rxAddr)
{
uint16_t crc = 0xb5d2;
if (rxAddr) {
for (int ii = 0; ii < RX_TX_ADDR_LEN; ++ii) {
crc = crc16_ccitt(crc, rxAddr[RX_TX_ADDR_LEN - 1 - ii]);
}
}
for (int ii = 0; ii < len; ++ii) {
crc = crc16_ccitt(crc, data[ii]);
data[ii] = bitReverse(data[ii] ^ xn297_data_scramble[ii]);
}
crc ^= xn297_crc_xorout[len];
return crc;
}
// Receive ISR callback
static void sumdDataReceive(uint16_t c, void *data)
{
UNUSED(data);
uint32_t sumdTime;
static uint32_t sumdTimeLast;
static uint8_t sumdIndex;
sumdTime = micros();
if ((sumdTime - sumdTimeLast) > 4000)
sumdIndex = 0;
sumdTimeLast = sumdTime;
if (sumdIndex == 0) {
if (c != SUMD_SYNCBYTE)
return;
else
{
sumdFrameDone = false; // lazy main loop didnt fetch the stuff
crc = 0;
}
}
if (sumdIndex == 2)
sumdChannelCount = (uint8_t)c;
if (sumdIndex < SUMD_BUFFSIZE)
sumd[sumdIndex] = (uint8_t)c;
sumdIndex++;
if (sumdIndex < sumdChannelCount * 2 + 4)
crc = crc16_ccitt(crc, (uint8_t)c);
else
if (sumdIndex == sumdChannelCount * 2 + 5) {
sumdIndex = 0;
sumdFrameDone = true;
}
}
int
nffs_crc_flash(uint16_t initial_crc, uint8_t area_idx, uint32_t area_offset,
uint32_t len, uint16_t *out_crc)
{
uint32_t chunk_len;
uint16_t crc;
int rc;
crc = initial_crc;
/* Copy data in chunks small enough to fit in the flash buffer. */
while (len > 0) {
if (len > sizeof nffs_flash_buf) {
chunk_len = sizeof nffs_flash_buf;
} else {
chunk_len = len;
}
rc = nffs_flash_read(area_idx, area_offset, nffs_flash_buf, chunk_len);
if (rc != 0) {
return rc;
}
crc = crc16_ccitt(crc, nffs_flash_buf, chunk_len);
area_offset += chunk_len;
len -= chunk_len;
}
*out_crc = crc;
return 0;
}
/*************************************************
* Function: AC_BuildMessage
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
void ICACHE_FLASH_ATTR
AC_BuildMessage(u8 u8MsgCode, u8 u8MsgId,
u8 *pu8Payload, u16 u16PayloadLen,
AC_OptList *pstruOptList,
u8 *pu8Msg, u16 *pu16Len)
{
//协议组包函数
ZC_MessageHead *pstruMsg = NULL;
u16 u16OptLen = 0;
u16 crc = 0;
pstruMsg = (ZC_MessageHead *)pu8Msg;
pstruMsg->MsgCode = u8MsgCode;
pstruMsg->MsgId = u8MsgId;
pstruMsg->Version = 0;
AC_BuildOption(pstruOptList, &pstruMsg->OptNum, (pu8Msg + sizeof(ZC_MessageHead)), &u16OptLen);
memcpy((pu8Msg + sizeof(ZC_MessageHead) + u16OptLen), pu8Payload, u16PayloadLen);
/*updata len*/
pstruMsg->Payloadlen = ZC_HTONS(u16PayloadLen + u16OptLen);
/*calc crc*/
crc = crc16_ccitt((pu8Msg + sizeof(ZC_MessageHead)), (u16PayloadLen + u16OptLen));
pstruMsg->TotalMsgCrc[0]=(crc&0xff00)>>8;
pstruMsg->TotalMsgCrc[1]=(crc&0xff);
*pu16Len = (u16)sizeof(ZC_MessageHead) + u16PayloadLen + u16OptLen;
}
/*************************************************
* Function: ZC_StoreTokenKey
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
void ZC_StoreTokenKey(u8 *pu8Data)
{
g_struZcConfigDb.struDeviceInfo.u32UnBcFlag = ZC_MAGIC_FLAG;
memcpy(g_struZcConfigDb.struCloudInfo.u8TokenKey, pu8Data, ZC_HS_SESSION_KEY_LEN);
g_struZcConfigDb.u32Crc = crc16_ccitt(((u8 *)&g_struZcConfigDb) + 4, sizeof(g_struZcConfigDb) - 4);
g_struProtocolController.pstruMoudleFun->pfunWriteFlash((u8*)&g_struZcConfigDb, sizeof(ZC_ConfigDB));
}
/*
* Write the configuration parameters to EEPROM
*/
void ConfigParms::commit(bool forced)
{
if (_stationName[0] == '\0') {
// It was probably not initialized.
return;
}
if (!forced && !needCommit) {
return;
}
//DIAGPRINTLN(F("ConfigParms::commit"));
// Fill in the magic and CRC, and write to EEPROM
const size_t crc_size = sizeof(uint16_t);
const size_t magic_len = sizeof(magic);
size_t size = magic_len + sizeof(*this) + crc_size;
uint8_t buffer[size];
strncpy_P((char *)buffer, magic, magic_len);
memcpy(buffer + magic_len, (uint8_t *)this, sizeof(*this));
uint16_t crc = crc16_ccitt(buffer, size - crc_size);
*(uint16_t *)(buffer + size - crc_size) = crc;
//dumpBuffer(buffer, size);
eeprom_write_block((const void *)buffer, eepromAddr(), size);
needCommit = false;
}
/*
* Read all of the config parameters from the EEPROM
*
* There are some sanity checks. If they fail then
* it will call reset() instead.
*/
void ConfigParms::read()
{
const size_t crc_size = sizeof(uint16_t);
const size_t magic_len = sizeof(magic);
size_t size = magic_len + sizeof(*this) + crc_size;
uint8_t buffer[size];
// Read the whole block from EEPROM in memory
eeprom_read_block((void *)buffer, eepromAddr(), size);
// Verify the checksum
uint16_t crc = *(uint16_t *)(buffer + size - crc_size);
uint16_t crc1 = crc16_ccitt(buffer, size - crc_size);
if (strncmp_P((const char *)buffer, magic, magic_len) != 0) {
//DIAGPRINTLN(F("ConfigParms::read - magic wrong"));
goto do_reset;
}
if (crc != crc1) {
goto do_reset;
}
// Initialize the ConfigParms instance with the info from EEPROM
memcpy((uint8_t *)this, buffer + magic_len, sizeof(*this));
return;
do_reset:
reset();
}
/*************************************************
* Function: ZC_ConfigReset
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
void ZC_ConfigReset()
{
g_struZcConfigDb.struSwitchInfo.u32ServerAddrConfig = 0;
g_struZcConfigDb.struDeviceInfo.u32UnBcFlag = 0xFFFFFFFF;
g_struZcConfigDb.u32Crc = crc16_ccitt(((u8 *)&g_struZcConfigDb) + 4, sizeof(g_struZcConfigDb) - 4);
g_struProtocolController.pstruMoudleFun->pfunWriteFlash((u8 *)&g_struZcConfigDb, sizeof(ZC_ConfigDB));
g_struProtocolController.pstruMoudleFun->pfunRest();
}
/*************************************************
* Function: ZC_StoreConnectionInfo
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
void ZC_StoreConnectionInfo(u8 *pu8Ssid, u8 *pu8Password)
{
g_struZcConfigDb.struConnection.u32MagicFlag = ZC_MAGIC_FLAG;
memcpy(g_struZcConfigDb.struConnection.u8Ssid, pu8Ssid, ZC_SSID_MAX_LEN);
memcpy(g_struZcConfigDb.struConnection.u8Password, pu8Password, ZC_PASSWORD_MAX_LEN);
g_struZcConfigDb.u32Crc = crc16_ccitt(((u8 *)&g_struZcConfigDb) + 4, sizeof(g_struZcConfigDb) - 4);
g_struProtocolController.pstruMoudleFun->pfunWriteFlash((u8*)&g_struZcConfigDb, sizeof(ZC_ConfigDB));
}
/*
get CRC for a block
*/
uint16_t AP_Terrain::get_block_crc(struct grid_block &block)
{
uint16_t saved_crc = block.crc;
block.crc = 0;
uint16_t ret = crc16_ccitt((const uint8_t *)&block, sizeof(block), 0);
block.crc = saved_crc;
return ret;
}
void crc16_ccitt_wd_buf(const uint8_t *in, uint len,
uint8_t *out, uint chunk_sz)
{
uint16_t crc;
crc = crc16_ccitt(0, in, len);
crc = htons(crc);
memcpy(out, &crc, sizeof(crc));
}
static int l_crc16(lua_State *L)
{
size_t size;
const char *p_str = luaL_checklstring(L, 1, &size);
uint16_t retval = crc16_ccitt( (uint8_t*) p_str, size);
lua_pushinteger(L, (int) retval);
return 1;
}
static uint16_t
nffs_crc_disk_inode_hdr(const struct nffs_disk_inode *disk_inode)
{
uint16_t crc;
crc = crc16_ccitt(0, disk_inode, NFFS_DISK_INODE_OFFSET_CRC);
return crc;
}
uint16_t
nffs_crc_disk_block_hdr(const struct nffs_disk_block *disk_block)
{
uint16_t crc;
crc = crc16_ccitt(0, disk_block, NFFS_DISK_BLOCK_OFFSET_CRC);
return crc;
}
/*************************************************
* Function: ZC_StoreConnectionInfo
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
void ZC_StoreAccessInfo(u8 *pu8ServerIp, u8 *pu8ServerPort)
{
g_struZcConfigDb.struConnection.u32MagicFlag = ZC_MAGIC_FLAG;
g_struZcConfigDb.struSwitchInfo.u32ServerAddrConfig = 1;
memcpy(&g_struZcConfigDb.struSwitchInfo.u32ServerIp, pu8ServerIp, ZC_SERVER_ADDR_MAX_LEN);
memcpy(&g_struZcConfigDb.struSwitchInfo.u16ServerPort, pu8ServerPort, ZC_SERVER_PORT_MAX_LEN);
g_struZcConfigDb.u32Crc = crc16_ccitt(((u8 *)&g_struZcConfigDb) + 4, sizeof(g_struZcConfigDb) - 4);
g_struProtocolController.pstruMoudleFun->pfunWriteFlash((u8*)&g_struZcConfigDb, sizeof(ZC_ConfigDB));
}
uint8_t XN297_WritePayload(uint8_t *data, int len, const uint8_t *rxAddr)
{
uint8_t packet[NRF24L01_MAX_PAYLOAD_SIZE];
uint16_t crc = 0xb5d2;
for (int ii = 0; ii < RX_TX_ADDR_LEN; ++ii) {
packet[ii] = rxAddr[RX_TX_ADDR_LEN - 1 - ii];
crc = crc16_ccitt(crc, packet[ii]);
}
for (int ii = 0; ii < len; ++ii) {
const uint8_t bOut = bitReverse(data[ii]);
packet[ii + RX_TX_ADDR_LEN] = bOut ^ xn297_data_scramble[ii];
crc = crc16_ccitt(crc, packet[ii + RX_TX_ADDR_LEN]);
}
crc ^= xn297_crc_xorout[len];
packet[RX_TX_ADDR_LEN + len] = crc >> 8;
packet[RX_TX_ADDR_LEN + len + 1] = crc & 0xff;
return NRF24L01_WritePayload(packet, RX_TX_ADDR_LEN + len + 2);
}
void crc16_ccitt_sbuf_append(sbuf_t *dst, uint8_t *start)
{
uint16_t crc = 0;
const uint8_t * const end = sbufPtr(dst);
for (const uint8_t *ptr = start; ptr < end; ++ptr) {
crc = crc16_ccitt(crc, *ptr);
}
sbufWriteU16(dst, crc);
}
guint16 crc16_ccitt_tvb_offset(tvbuff_t *tvb, guint offset, guint len)
{
const guint8 *buf;
tvb_ensure_bytes_exist(tvb, offset, len); /* len == -1 not allowed */
buf = tvb_get_ptr(tvb, offset, len);
return crc16_ccitt(buf, len);
}
inline void crc16_ccitt_readReply(unsigned int numDataBytes)
{
// shift everything over by 1 to accomodate leading "0" bit.
// first, grab address of beginning of array
readReply[numDataBytes + 2] = 0; // clear out this spot for the loner bit of
// handle
readReply[numDataBytes + 4] = 0; // clear out this spot for the loner bit of
// crc
bits = (unsigned short) &readReply[0];
// shift all bytes and later use only data + handle
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
asm("RRC.b @R5+");
// store loner bit in array[numDataBytes+2] position
asm("RRC.b @R5+");
// make first bit 0
readReply[0] &= 0x7f;
// compute crc on data + handle bytes
readReplyCRC = crc16_ccitt(&readReply[0], numDataBytes + 2);
readReply[numDataBytes + 4] = readReply[numDataBytes + 2];
// XOR the MSB of CRC with loner bit.
readReply[numDataBytes + 4] ^= __swap_bytes(readReplyCRC); // XOR happens with
// MSB of lower
// nibble
// Just take the resulting bit, not the whole byte
readReply[numDataBytes + 4] &= 0x80;
unsigned short mask = __swap_bytes(readReply[numDataBytes + 4]);
mask >>= 3;
mask |= (mask >> 7);
mask ^= 0x1020;
mask >>= 1; // this is because the loner bit pushes the CRC to the left by 1
// but we don't shift the crc because it should get pushed out by 1 anyway
readReplyCRC ^= mask;
readReply[numDataBytes + 3] = (unsigned char) readReplyCRC;
readReply[numDataBytes + 2] |= (unsigned char) (__swap_bytes(readReplyCRC) &
0x7F);
}
void
nffs_crc_disk_block_fill(struct nffs_disk_block *disk_block, const void *data)
{
uint16_t crc16;
crc16 = nffs_crc_disk_block_hdr(disk_block);
crc16 = crc16_ccitt(crc16, data, disk_block->ndb_data_len);
disk_block->ndb_crc16 = crc16;
}
uint16_t crc16_ccitt_update(uint16_t crc, const void *data, uint32_t length)
{
const uint8_t *p = (const uint8_t *)data;
const uint8_t *pend = p + length;
for (; p != pend; p++) {
crc = crc16_ccitt(crc, *p);
}
return crc;
}
void
nffs_crc_disk_inode_fill(struct nffs_disk_inode *disk_inode,
const char *filename)
{
uint16_t crc16;
crc16 = nffs_crc_disk_inode_hdr(disk_inode);
crc16 = crc16_ccitt(crc16, filename, disk_inode->ndi_filename_len);
disk_inode->ndi_crc16 = crc16;
}
/** Send SBP messages.
* Takes an SBP message payload, type and sender ID then writes a message to
* the output stream using the supplied `write` function with the correct
* framing and CRC.
*
* The supplied `write` function must have the prototype:
*
* ~~~
* u32 write(u8 *buff, u32 n, void* context)
* ~~~
*
* where `n` is the number of bytes to be written and `buff` is the buffer from
* which to read the data to be written, and `context` is the arbitrary pointer
* set by `sbp_state_set_io_context`. The function should return the number
* of bytes successfully written which may be between 0 and `n`. Currently, if
* the number of bytes written is different from `n` then `sbp_send_message`
* will immediately return with an error.
*
* Note that `sbp_send_message` makes multiple calls to write and therefore if
* a `write` call fails then this may result in a partial message being written
* to the output. This should be caught by the CRC check on the receiving end
* but will result in lost messages.
*
* \param write Function pointer to a function that writes `n` bytes from
* `buff` to the output stream and returns the number of bytes
* successfully written.
* \return `SBP_OK` (0) if successful, `SBP_WRITE_ERROR` if the message could
* not be sent or was only partially sent.
*/
s8 sbp_send_message(sbp_state_t *s, u16 msg_type, u16 sender_id, u8 len, u8 *payload,
u32 (*write)(u8 *buff, u32 n, void *context))
{
/* Check our payload data pointer isn't NULL unless len = 0. */
if (len != 0 && payload == 0)
return SBP_NULL_ERROR;
/* Check our write function pointer isn't NULL. */
if (write == 0)
return SBP_NULL_ERROR;
u16 crc;
u8 preamble = SBP_PREAMBLE;
if ((*write)(&preamble, 1, s->io_context) != 1)
return SBP_SEND_ERROR;
if ((*write)((u8*)&msg_type, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
if ((*write)((u8*)&sender_id, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
if ((*write)(&len, 1, s->io_context) != 1)
return SBP_SEND_ERROR;
if (len > 0) {
if ((*write)(payload, len, s->io_context) != len)
return SBP_SEND_ERROR;
}
crc = crc16_ccitt((u8*)&(msg_type), 2, 0);
crc = crc16_ccitt((u8*)&(sender_id), 2, crc);
crc = crc16_ccitt(&(len), 1, crc);
crc = crc16_ccitt(payload, len, crc);
if ((*write)((u8*)&crc, 2, s->io_context) != 2)
return SBP_SEND_ERROR;
return SBP_OK;
}
/*************************************************
* Function: PCT_ModuleOtaFileChunkMsg
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
void PCT_ModuleOtaFileChunkMsg(PTC_ProtocolCon *pstruContoller, ZC_MessageHead *pstruMsg, u8 *pu8Msg)
{
ZC_OtaFileChunkReq *pstruOta;
u32 u32FileLen;
u32 u32RetVal;
u32 u32RecvOffset;
u16 u16CalCrc;
u16 u16RecvCrc;
ZC_Printf("Ota File Chunk\n");
pstruOta = (ZC_OtaFileChunkReq *)(pu8Msg);
u32FileLen = ZC_HTONS(pstruMsg->Payloadlen) - sizeof(ZC_OtaFileChunkReq);
u32RecvOffset = ZC_HTONL(pstruOta->u32Offset);
/*check para*/
if ((u32RecvOffset != pstruContoller->struOtaInfo.u32RecvOffset)
|| ((u32RecvOffset + u32FileLen) > pstruContoller->struOtaInfo.u32TotalLen)
|| (u32FileLen > ZC_OTA_MAX_CHUNK_LEN))
{
ZC_Printf("recv error %d,%d,%d,%d,\n", u32RecvOffset,
pstruContoller->struOtaInfo.u32RecvOffset,
pstruContoller->struOtaInfo.u32TotalLen,
u32FileLen);
PCT_SendNotifyMsg(ZC_CODE_ERR);
return;
}
//Check CRC
u16CalCrc = crc16_ccitt((u8*)(pstruOta), ZC_HTONS(pstruMsg->Payloadlen));
u16RecvCrc = (pstruMsg->TotalMsgCrc[0] << 8) + pstruMsg->TotalMsgCrc[1];
if (u16CalCrc != u16RecvCrc)
{
PCT_SendNotifyMsg(ZC_CODE_ERR);
return;
}
u32RetVal = pstruContoller->pstruMoudleFun->pfunUpdate((u8*)(pstruOta + 1), u32RecvOffset, u32FileLen);
//u32RetVal = ZC_RET_OK;
ZC_Printf("offset = %d, len = %d\n", u32RecvOffset, u32FileLen);
if (ZC_RET_ERROR == u32RetVal)
{
ZC_Printf("OTA Fail\n");
PCT_SendNotifyMsg(ZC_CODE_ERR);
return;
}
/*update file offset*/
pstruContoller->struOtaInfo.u32RecvOffset = pstruContoller->struOtaInfo.u32RecvOffset + u32FileLen;
PCT_SendNotifyMsg(ZC_CODE_ACK);
}
static int
nffs_write_fill_crc16_overwrite(struct nffs_disk_block *disk_block,
uint8_t src_area_idx, uint32_t src_area_offset,
uint16_t left_copy_len, uint16_t right_copy_len,
const void *new_data, uint16_t new_data_len)
{
uint16_t block_off;
uint16_t crc16;
int rc;
block_off = 0;
crc16 = nffs_crc_disk_block_hdr(disk_block);
block_off += sizeof *disk_block;
/* Copy data from the start of the old block, in case the new data starts
* at a non-zero offset.
*/
if (left_copy_len > 0) {
rc = nffs_crc_flash(crc16, src_area_idx, src_area_offset + block_off,
left_copy_len, &crc16);
if (rc != 0) {
return rc;
}
block_off += left_copy_len;
}
/* Write the new data into the data block. This may extend the block's
* length beyond its old value.
*/
crc16 = crc16_ccitt(crc16, new_data, new_data_len);
block_off += new_data_len;
/* Copy data from the end of the old block, in case the new data doesn't
* extend to the end of the block.
*/
if (right_copy_len > 0) {
rc = nffs_crc_flash(crc16, src_area_idx, src_area_offset + block_off,
right_copy_len, &crc16);
if (rc != 0) {
return rc;
}
block_off += right_copy_len;
}
assert(block_off == sizeof *disk_block + disk_block->ndb_data_len);
disk_block->ndb_crc16 = crc16;
return 0;
}
int stream_next_bit(struct stream *s)
{
int b;
if (s->nr_index >= 5)
return -1;
s->index_offset++;
if ((b = s->type->next_bit(s)) == -1)
return -1;
s->word = (s->word << 1) | b;
if (++s->crc_bitoff == 16) {
uint8_t b = mfm_decode_bits(bc_mfm, s->word);
s->crc16_ccitt = crc16_ccitt(&b, 1, s->crc16_ccitt);
s->crc_bitoff = 0;
}
return b;
}
void printDebugPacket() {
u16 crc = crc16_ccitt(pktbuf, 6);
SSN = LOW;
setCursor(1, 30);
printf("%02x %02x %02x %02x", pktbuf[0], pktbuf[1], pktbuf[2], pktbuf[3]);
setCursor(2, 30);
printf("%02x %02x %02x %02x", pktbuf[4], pktbuf[5], pktbuf[6], pktbuf[7]);
setCursor(3, 24);
printf("%04x", crc);
setCursor(4, 24);
printf("%3u ", pktbuf[9]);
setCursor(5, 30);
printf("%3u ", pktbuf[8]);
setCursor(6, 42);
printf("%3u ", pktbuf[10]);
SSN= HIGH;
}
/*************************************************
* Function: PCT_CheckCrc
* Description:
* Author: cxy
* Returns:
* Parameter:
* History:
*************************************************/
u32 PCT_CheckCrc(u8 *pu8Crc, u8 *pu8Data, u16 u16Len)
{
u16 u16Crc;
u16 u16RecvCrc;
u16Crc = crc16_ccitt(pu8Data, u16Len);
u16RecvCrc = (pu8Crc[0] << 8) + pu8Crc[1];
if (u16Crc == u16RecvCrc)
{
return ZC_RET_OK;
}
else
{
ZC_Printf("Crc is error, recv = %d, calc = %d\n", u16RecvCrc, u16Crc);
return ZC_RET_ERROR;
}
}
void assemblyPacket(packet_type_e command, const packet_data_u *data, uint8_t packet[MAX_PACKET_SIZE], uint8_t *packetSize)
{
// Fill header
packet_header *header = (packet_header *)packet;
memset(header, 0x00, HEADER_LENGTH);
header->preamble = PREAMBLE;
header->direction = DIRECTION_FROM_DEVICE;
header->encrypted = ENCRYPTION_OFF;
header->type = command;
header->reserved = 0xFFFFFFFF;
// Copy data in packet
uint8_t *dataField = &packet[10];
unsigned int dataFieldSize = 0;
switch (command) {
case GET_DEVICE_ID:
dataFieldSize = 8;
memcpy(dataField, data->device_id, dataFieldSize);
break;
case GET_STATUS: {
dataFieldSize = sizeof(device_status_s);
memcpy(dataField, &data->device_status, dataFieldSize);
break;
}
case GET_BUTTONS_STATE:
dataFieldSize = 3 * sizeof(button_state_s);
memcpy(dataField, data->buttons_state, dataFieldSize);
break;
default:
*packetSize = 0;
return;
}
// Calculate packet size
header->length = 6 + dataFieldSize;
*packetSize = 4 + header->length + CRC_LEN;
// Calculate CRC over: length, encrypted, type, reserved, data
uint16_t crc;
crc = crc16_ccitt(&packet[2], (2 + header->length));
memcpy(packet + HEADER_LENGTH + dataFieldSize, &crc, CRC_LEN);
}