327 lines
9.7 KiB
C
327 lines
9.7 KiB
C
/****************************************************************************
|
|
|
|
Copyright(c) 2019 by Aerospace C.Power (Chongqing) Microelectronics. ALL RIGHTS RESERVED.
|
|
|
|
This Information is proprietary to Aerospace C.Power (Chongqing) Microelectronics and MAY NOT
|
|
be copied by any method or incorporated into another program without
|
|
the express written consent of Aerospace C.Power. This Information or any portion
|
|
thereof remains the property of Aerospace C.Power. The Information contained herein
|
|
is believed to be accurate and Aerospace C.Power assumes no responsibility or
|
|
liability for its use in any way and conveys no license or title under
|
|
any patent or copyright and makes no representation or warranty that this
|
|
Information is free from patent or copyright infringement.
|
|
|
|
****************************************************************************/
|
|
|
|
/* common includes */
|
|
#include "iot_errno_api.h"
|
|
#include "iot_sg_fr.h"
|
|
|
|
/* protocol includes */
|
|
#include "proto_spg.h"
|
|
#include "iot_sg_ext_api.h"
|
|
#include "iot_cli_sg_api.h"
|
|
|
|
static const uint8_t spg_bcast_mac[IOT_MAC_ADDR_LEN] =
|
|
{ 0x99,0x99,0x99,0x99,0x99,0x99 };
|
|
|
|
uint8_t proto_spg_preamble[PROTO_SPG_PREAMBLE_LEN] =
|
|
{ 0xFE, 0xFE, 0xFE, 0xFE };
|
|
|
|
uint32_t proto_spg_get_proto_pkt_len(uint8_t mac_used, uint32_t data_len)
|
|
{
|
|
uint32_t tmp_len = 0;
|
|
/* IOT_SG_EXT_HEADROOM is additional uart data */
|
|
tmp_len = data_len + PROTO_SPG_FIX_FIELD_LEN +
|
|
sizeof(proto_spg_app_data_t);
|
|
|
|
if (mac_used) {
|
|
tmp_len += (PROTO_SPG_MAC_ADDR_LEN << 1);
|
|
}
|
|
return tmp_len;
|
|
}
|
|
|
|
uint32_t proto_spg_get_pkt_len(uint8_t mac_used, uint32_t data_len)
|
|
{
|
|
return max(IOT_SG_EXT_HEADROOM, IOT_SG_CLI_CHANNEL_HEADROOM) +
|
|
proto_spg_get_proto_pkt_len(mac_used, data_len);
|
|
}
|
|
|
|
uint32_t proto_spg_get_header_len(uint8_t mac_used)
|
|
{
|
|
uint32_t tmp_len = 0;
|
|
|
|
tmp_len = PROTO_SPG_DATA_OFFSET + sizeof(proto_spg_app_data_t);
|
|
|
|
if (mac_used) {
|
|
tmp_len += (PROTO_SPG_MAC_ADDR_LEN << 1);
|
|
}
|
|
return tmp_len;
|
|
}
|
|
|
|
uint32_t proto_spg_get_rsvd_len(uint8_t mac_used)
|
|
{
|
|
/* IOT_SG_EXT_HEADROOM is additional uart data */
|
|
return max(IOT_SG_EXT_HEADROOM, IOT_SG_CLI_CHANNEL_HEADROOM) +
|
|
proto_spg_get_header_len(mac_used);
|
|
}
|
|
|
|
uint8_t proto_spg_get_checksum(uint8_t *data, uint32_t len)
|
|
{
|
|
uint8_t result = 0;
|
|
for (uint32_t i = 0; i < len; ++i) {
|
|
result += data[i];
|
|
}
|
|
return result;
|
|
}
|
|
|
|
void proto_spg_nid_to_buf(uint32_t nid, uint8_t *p_nid)
|
|
{
|
|
p_nid[0] = (uint8_t)nid;
|
|
p_nid[1] = (uint8_t)(nid >> 8);
|
|
p_nid[2] = (uint8_t)(nid >> 16);
|
|
}
|
|
|
|
uint16_t proto_spg_fill_frame(uint8_t *frames_filled, uint8_t role, uint8_t afn,
|
|
uint8_t fn, uint8_t di2, uint8_t di3, uint8_t seq, uint8_t *app_data,
|
|
uint16_t data_len, uint8_t *mac_buf, uint8_t mac_cnt, uint8_t dir)
|
|
{
|
|
uint8_t *data = NULL;
|
|
uint16_t index = 0;
|
|
uint8_t *cs_ptr = NULL; //pointer to checksum
|
|
proto_spg_hdr_t *pkt_hdr = NULL;
|
|
proto_spg_app_data_t *spg_data = NULL;
|
|
|
|
data = frames_filled;
|
|
pkt_hdr = (proto_spg_hdr_t*)data;
|
|
pkt_hdr->sof_byte = PROTO_SPG_SOF_BYTE;
|
|
pkt_hdr->ctrl_byte.dir = dir;
|
|
pkt_hdr->ctrl_byte.prm = role;
|
|
pkt_hdr->ctrl_byte.ver = 0;
|
|
pkt_hdr->ctrl_byte.rsvd = 0;
|
|
index += sizeof(proto_spg_hdr_t);
|
|
if (mac_cnt) {
|
|
pkt_hdr->ctrl_byte.addr = 1;
|
|
/* only src mac and dest mac for up link */
|
|
os_mem_cpy(data + index, mac_buf, mac_cnt * PROTO_SPG_MAC_ADDR_LEN);
|
|
index += mac_cnt * PROTO_SPG_MAC_ADDR_LEN;
|
|
}
|
|
spg_data = (proto_spg_app_data_t *)(data + index);
|
|
/* set AFN */
|
|
spg_data->afn = afn;
|
|
/* set seq */
|
|
spg_data->seq = seq;
|
|
/* set DI */
|
|
spg_data->di[0] = fn;
|
|
spg_data->di[1] = afn;
|
|
spg_data->di[2] = di2;
|
|
spg_data->di[3] = di3;
|
|
index += sizeof(proto_spg_app_data_t);
|
|
|
|
/* skip app data as app data is already in iot_pkt */
|
|
if (data_len && app_data) {
|
|
os_mem_cpy(spg_data->data, app_data, data_len);
|
|
index += data_len;
|
|
}
|
|
cs_ptr = &data[index++]; /* set the checksum byte pointer */
|
|
data[index++] = PROTO_SPG_EOF_BYTE;
|
|
pkt_hdr->len = index;
|
|
*cs_ptr = proto_spg_get_checksum(data + PROTO_SPG_CTRL_OFFSET,
|
|
index - PROTO_SPG_PRECODE_LEN - PROTO_SPG_LENGTH_SIZE - \
|
|
PROTO_SPG_CHECKSUM_LEN - PROTO_SPG_BACKCODE_LEN);
|
|
return index;
|
|
}
|
|
|
|
iot_pkt_t *proto_spg_alloc_frame(uint8_t role, uint8_t afn, uint8_t fn,
|
|
uint8_t di2, uint8_t di3, uint8_t seq, uint8_t *app_data, uint16_t len,
|
|
uint8_t *mac_buf, uint8_t mac_cnt, uint8_t dir)
|
|
{
|
|
iot_pkt_t *pkt;
|
|
uint16_t pkt_len;
|
|
pkt = iot_pkt_alloc(proto_spg_get_pkt_len(mac_cnt, len),
|
|
IOT_SMART_GRID_MID);
|
|
if (pkt == NULL) {
|
|
goto out;
|
|
}
|
|
pkt_len = proto_spg_fill_frame(iot_pkt_data(pkt), role, afn, fn, di2, di3,
|
|
seq, app_data, len, mac_buf, mac_cnt, dir);
|
|
iot_pkt_put(pkt, pkt_len);
|
|
out:
|
|
return pkt;
|
|
}
|
|
|
|
uint8_t *proto_spg_sanity_check(uint8_t *data, uint16_t len,
|
|
uint8_t *reason, uint8_t di3)
|
|
{
|
|
uint16_t len_t = len, i, frame_len, min_data_len = PROTO_SPG_FIX_FIELD_LEN;
|
|
uint8_t *data_s = data;
|
|
proto_spg_hdr_t *proto_hdr;
|
|
proto_spg_app_data_t* app_data;
|
|
again:
|
|
len = len_t;
|
|
for (i = 0x0; i < len; i++) {
|
|
if (data_s[i] == PROTO_SPG_SOF_BYTE) {
|
|
data_s += i;
|
|
len_t -= i;
|
|
break;
|
|
}
|
|
}
|
|
if (i == len) {
|
|
goto drop;
|
|
}
|
|
if (len_t <= min_data_len) {
|
|
goto drop;
|
|
}
|
|
frame_len = (uint16_t)data_s[2] << 8 | (uint16_t)data_s[1];
|
|
if (frame_len > len_t) {
|
|
data_s++;
|
|
len_t--;
|
|
goto again;
|
|
}
|
|
if ((data_s[0] != PROTO_SPG_SOF_BYTE)
|
|
|| (data_s[frame_len - 1] != PROTO_SPG_EOF_BYTE)) {
|
|
data_s++;
|
|
len_t--;
|
|
goto again;
|
|
}
|
|
proto_hdr = (proto_spg_hdr_t *)data_s;
|
|
if (proto_hdr->ctrl_byte.addr) {
|
|
if (len_t < (PROTO_SPG_PKT_OVERHEAD +
|
|
(PROTO_SPG_MAC_ADDR_LEN * 2))) {
|
|
goto drop;
|
|
}
|
|
app_data = (proto_spg_app_data_t*)(data_s + sizeof(proto_spg_hdr_t)
|
|
+ sizeof(proto_spg_addr_field_t));
|
|
} else {
|
|
app_data = (proto_spg_app_data_t*)(data_s + sizeof(proto_spg_hdr_t));
|
|
}
|
|
if (di3 != app_data->di[3]) {
|
|
data_s++;
|
|
len_t--;
|
|
goto again;
|
|
}
|
|
if (data_s[frame_len - 2] != proto_spg_get_checksum(
|
|
data_s + PROTO_SPG_CTRL_OFFSET,
|
|
frame_len - PROTO_SPG_FIX_FIELD_LEN + 1)) {
|
|
data_s++;
|
|
len_t--;
|
|
goto again;
|
|
}
|
|
goto out;
|
|
drop:
|
|
*reason = PROTO_SPG_ERR_INVALID_DATA;
|
|
data_s = NULL;
|
|
out:
|
|
return data_s;
|
|
}
|
|
|
|
void proto_spg_check_frame_handler(uint8_t* buffer, uint32_t buffer_len,
|
|
bool_t* is_frame)
|
|
{
|
|
uint16_t frame_len = 0;
|
|
proto_spg_hdr_t *proto_hdr;
|
|
proto_spg_app_data_t* app_data;
|
|
do {
|
|
if (buffer_len < PROTO_SPG_PKT_OVERHEAD) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
|
|
frame_len = (uint16_t)buffer[2];
|
|
frame_len = (frame_len << 8) | (uint16_t)buffer[1];
|
|
|
|
if (buffer[0] != PROTO_SPG_SOF_BYTE
|
|
|| buffer[buffer_len - 1] != PROTO_SPG_EOF_BYTE) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
|
|
if (frame_len != buffer_len) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
|
|
proto_hdr = (proto_spg_hdr_t*)buffer;
|
|
if (proto_hdr->ctrl_byte.addr) {
|
|
if (buffer_len < (PROTO_SPG_PKT_OVERHEAD +
|
|
(PROTO_SPG_MAC_ADDR_LEN * 2))) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
app_data = (proto_spg_app_data_t*)(buffer + sizeof(proto_spg_hdr_t)
|
|
+ sizeof(proto_spg_addr_field_t));
|
|
} else {
|
|
app_data = (proto_spg_app_data_t*)(buffer + sizeof(proto_spg_hdr_t));
|
|
}
|
|
#if PLC_SUPPORT_CCO_ROLE
|
|
if (app_data->di[3] != 0xE8) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
#elif (IOT_SG_CONTROLLER_ENABLE && (IOT_STA_CONTROL_MODE == 0))
|
|
if (app_data->di[3] != 0xE5) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
#else
|
|
if (app_data->di[3] != 0xEA) {
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
#endif /* PLC_SUPPORT_CCO_ROLE */
|
|
|
|
if (buffer[buffer_len - 2] != proto_spg_get_checksum(
|
|
buffer + PROTO_SPG_CTRL_OFFSET,
|
|
buffer_len - PROTO_SPG_FIX_FIELD_LEN + 1)) {
|
|
/* checksum failure */
|
|
*is_frame = false;
|
|
break;
|
|
}
|
|
*is_frame = true;
|
|
} while(0);
|
|
}
|
|
|
|
uint8_t proto_spg_is_bcast(uint8_t* dst)
|
|
{
|
|
return iot_mac_addr_cmp(dst, spg_bcast_mac);
|
|
}
|
|
|
|
void proto_spg_set_bcast(uint8_t *dst)
|
|
{
|
|
iot_mac_addr_cpy(dst, (uint8_t*)spg_bcast_mac);
|
|
}
|
|
|
|
static uint32_t proto_spg_get_block_cnt(uint32_t file_size,
|
|
uint16_t block_size)
|
|
{
|
|
return (0 == block_size) ? 0 : ((file_size + block_size - 1) / block_size);
|
|
}
|
|
|
|
uint32_t proto_spg_get_block_size(uint32_t file_size, uint16_t block_cnt)
|
|
{
|
|
uint32_t cnt = 0;
|
|
|
|
cnt = proto_spg_get_block_cnt(file_size, PROTO_SPG_UPGRADE_FILE_SEG_SIZE1);
|
|
if (block_cnt == (uint16_t)cnt) {
|
|
return PROTO_SPG_UPGRADE_FILE_SEG_SIZE1;
|
|
}
|
|
cnt = proto_spg_get_block_cnt(file_size, PROTO_SPG_UPGRADE_FILE_SEG_SIZE2);
|
|
if (block_cnt == (uint16_t)cnt) {
|
|
return PROTO_SPG_UPGRADE_FILE_SEG_SIZE2;
|
|
}
|
|
cnt = proto_spg_get_block_cnt(file_size, PROTO_SPG_UPGRADE_FILE_SEG_SIZE3);
|
|
if (block_cnt == (uint16_t)cnt) {
|
|
return PROTO_SPG_UPGRADE_FILE_SEG_SIZE3;
|
|
}
|
|
cnt = proto_spg_get_block_cnt(file_size, PROTO_SPG_UPGRADE_FILE_SEG_SIZE4);
|
|
if (block_cnt == (uint16_t)cnt) {
|
|
return PROTO_SPG_UPGRADE_FILE_SEG_SIZE4;
|
|
}
|
|
cnt = proto_spg_get_block_cnt(file_size, PROTO_SPG_UPGRADE_FILE_SEG_SIZE5);
|
|
if (block_cnt == (uint16_t)cnt) {
|
|
return PROTO_SPG_UPGRADE_FILE_SEG_SIZE5;
|
|
}
|
|
|
|
return 0;
|
|
}
|