/****************************************************************************

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.

****************************************************************************/

/* os shim includes */
#include "os_types_api.h"
#include "os_mem_api.h"

/* common includes */
#include "iot_utils_api.h"
#include "iot_errno_api.h"
#include "iot_bitmap_api.h"


/* smart grid internal includes */
#include "proto_645.h"
#include "proto_645_vendor.h"
#include "proto_645_topo.h"

uint8_t proto_645_bcast_addr[IOT_MAC_ADDR_LEN] =
    { 0x99, 0x99, 0x99, 0x99, 0x99, 0x99 };

uint8_t proto_645_bcast_dec_addr[IOT_MAC_ADDR_LEN] =
    { 99, 99, 99, 99, 99, 99 };

uint8_t proto_645_any_addr[IOT_MAC_ADDR_LEN] =
    { 0xAA, 0xAA, 0xAA, 0xAA, 0xAA, 0xAA };

uint8_t proto_645_preamble[PROTO_645_PREAMBLE_LEN] =
    { 0xFE, 0xFE, 0xFE, 0xFE };

uint8_t proto_645_calc_cs(proto_645_header_t *hdr)
{
    uint8_t ret = 0;
    uint16_t i, len;
    uint8_t *data = (uint8_t *)hdr;

    len = sizeof(*hdr) + hdr->len;
    for (i = 0; i < len; i++)
        ret += data[i];

    return ret;
}

proto_645_header_t *proto_645_format_check(uint8_t *data,
    uint32_t len, uint32_t dir)
{
    uint32_t len_t, d_len, i = 0x0;
    uint8_t *data_s = NULL;
    proto_645_header_t *head;
    proto_645_tailer_t *tail;
    for (i = 0x0, len_t = len; i < len; i++) {
        if (data[i] == PROTO_645_START_CHAR) {
            data_s = data + i;
            break;
        }
        len_t--;
    }
    if (data_s == NULL
        || len_t < sizeof(proto_645_header_t)) {
        goto drop;
    }
    head = (proto_645_header_t *)data_s;
    if (head->start_char_1 != PROTO_645_START_CHAR
        || head->start_char_2 != PROTO_645_START_CHAR) {
        goto drop;
    }
    d_len = head->len;
    if (len_t < (sizeof(proto_645_header_t) + d_len + \
        sizeof(proto_645_tailer_t))) {
        goto drop;
    }
    tail = (proto_645_tailer_t*)(data_s + sizeof(*head) + d_len);
    if (proto_645_calc_cs(head) != tail->cs) {
        goto drop;
    }
    if (tail->end_char != PROTO_645_END_CHAR)
        goto drop;

    if (head->control.dir != dir) {
        goto drop;
    }
    goto out;
drop:
    data_s = NULL;
out:
    return (proto_645_header_t *)data_s;
}

uint32_t proto_645_get_read_rsp_len(uint8_t p_id, uint32_t di)
{
    uint32_t d_len = 0;
    uint32_t di_len;
    uint32_t fixed_len = sizeof(proto_645_header_t) + \
        sizeof(proto_645_tailer_t);
    if (p_id == PROTO_645_2007_ID) {
        di_len = PROTO_645_2007_DI_LEN;
        switch (di) {
        case PROTO_645_2007_DI_FOR_WATTH_T:
            d_len = PROTO_645_2007_DI_FOR_WATTH_T_LEN;
            break;
        case PROTO_645_2007_DI_R_ADDR:
            d_len = PROTO_645_2007_DI_R_ADDR_LEN;
            break;
        default:
            d_len= (PROTO_645_READ_DATA_MAX_LEN - di_len);
            break;
        }
    } else {
        di_len = PROTO_645_1997_DI_LEN;
        switch (di) {
        case PROTO_645_1997_DI_EPT_POS_SUM:
            d_len = PROTO_645_1997_DI_EPT_POS_SUM_LEN;
            break;
        case PROTO_645_1997_DI_R_ADDR:
            d_len = PROTO_645_1997_DI_R_ADDR_LEN;
            break;
        default:
            d_len = (PROTO_645_READ_DATA_MAX_LEN - di_len);
            break;
        }
    }
    return (d_len + di_len + fixed_len);
}

void proto_645_add33_handle(uint8_t *ds, uint32_t size)
{
    uint8_t  *ptr = ds;
    while (size--)
    {
        (*ptr++) += 0x33;
    }
    return;
}

void proto_645_sub33_handle(uint8_t *ds, uint32_t size)
{
    uint8_t  *ptr = ds;
    while (size--)
    {
        (*ptr++) -= 0x33;
    }
    return;
}

void proto_645_invert_handle(uint8_t *ds, uint32_t size)
{
    uint8_t  *ptr = ds;
    while (size--)
    {
        (*ptr) = ~(*ptr);
        ptr++;
    }
    return;
}

void proto_645_header_init(proto_645_header_t *hdr,
    uint8_t *addr, uint8_t fn, uint8_t dir, uint8_t ack_v, uint8_t follow_v)
{
    IOT_ASSERT(addr);
    IOT_ASSERT(ack_v <= PROTO_645_ACK_ABNORMAL);
    IOT_ASSERT(follow_v <= PROTO_645_FOLLOW_AVAILABLE);
    IOT_ASSERT(dir <= PROTO_645_DIR_SLAVE);
    hdr->start_char_1 = PROTO_645_START_CHAR;
    iot_mac_addr_cpy(hdr->addr, addr);
    hdr->len = 0;
    hdr->control.fn  = fn;
    hdr->control.dir = dir;
    hdr->control.ack = ack_v;
    hdr->control.follow = follow_v;
    hdr->start_char_2 = PROTO_645_START_CHAR;
}

void proto_645_tail_init(proto_645_header_t *hdr)
{
    uint8_t *data = hdr->data, len = hdr->len;
    proto_645_tailer_t *tail = (proto_645_tailer_t*)(data + len);

    tail->cs = proto_645_calc_cs(hdr);
    tail->end_char = PROTO_645_END_CHAR;
}

uint32_t proto_645_fill_frame(uint8_t *frames_filled,
    uint8_t protocol_type, uint8_t *addr, uint8_t dir, uint8_t ack,
    uint8_t follow, uint8_t fn, uint32_t di, uint8_t payload_len,
    uint8_t *payload)
{
    uint8_t *data;
    uint32_t length = 0;
    proto_645_header_t *hdr;
    uint8_t di_len = 0;
    IOT_ASSERT(frames_filled && addr);
    data = frames_filled;
    hdr = (proto_645_header_t *)data;
    proto_645_header_init(hdr, addr, fn, dir, ack, follow);
    hdr->len = 0;
    if (di != PROTO_645_INVALID_DI) {
        /* fill DI */
        if (protocol_type == PROTO_645_2007_ID) {
            proto_645_2007_di_to_byte(di, hdr->data);
            hdr->len += PROTO_645_2007_DI_LEN;
            di_len = PROTO_645_2007_DI_LEN;
        } else {
            proto_645_1997_di_to_byte((uint16_t)di, hdr->data);
            hdr->len += PROTO_645_1997_DI_LEN;
            di_len = PROTO_645_1997_DI_LEN;
        }
    }
    if (payload && payload_len) {
        /* fill data */
        os_mem_cpy(hdr->data + di_len, payload, payload_len);
        hdr->len += payload_len;
    }
    proto_645_add33_handle(hdr->data, hdr->len);
    /* fill tail */
    proto_645_tail_init(hdr);
    length = (sizeof(proto_645_header_t) + hdr->len +
        sizeof(proto_645_tailer_t));
    return length;
}

/* create a meter correcting time message for the 645 protocol  */
iot_pkt_t *proto_645_build_corr_msg(uint8_t *addr, proto_645_corr_time_t *time)
{
    uint8_t *data, pm_addr[IOT_MAC_ADDR_LEN];
    iot_pkt_t *pkt;
    proto_645_corr_time_t *tm;
    proto_645_header_t *hdr;

    pkt = iot_pkt_alloc(PROTO_645_CORRECT_DATA_PKT_LEN + \
        PROTO_645_PREAMBLE_LEN, IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    iot_pkt_reserve(pkt, PROTO_645_PREAMBLE_LEN);
    data = iot_pkt_data(pkt);
    if (!addr) {
        iot_mac_addr_cpy(pm_addr, proto_645_bcast_addr);
    } else {
        iot_mac_addr_cpy(pm_addr, addr);
    }
    hdr = (proto_645_header_t*)data;
    proto_645_header_init(hdr, pm_addr, PROTO_645_2007_FN_CORRECT_TIME,
        PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID);
    hdr->len = sizeof(*tm);
    tm = (proto_645_corr_time_t*)hdr->data;
    os_mem_cpy(tm, time, sizeof(*tm));
    proto_645_add33_handle(hdr->data, hdr->len);
    proto_645_tail_init(hdr);
    iot_pkt_put(pkt, PROTO_645_CORRECT_DATA_PKT_LEN);
    return pkt;
}

/* create a meter frozen message for dl/t645 - 07 protocol */
iot_pkt_t *proto_645_2007_build_frozen_msg(uint8_t *addr,
    proto_645_07_frozen_mdhm_t *time)
{
    uint8_t *data;
    iot_pkt_t *pkt;
    proto_645_07_frozen_mdhm_t *tm;
    proto_645_header_t *hdr;

    pkt = iot_pkt_alloc(PROTO_645_2007_FROZEN_DATA_PKT_LEN +
        PROTO_645_PREAMBLE_LEN, IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    iot_pkt_reserve(pkt, PROTO_645_PREAMBLE_LEN);
    data = iot_pkt_data(pkt);
    hdr = (proto_645_header_t*)data;
    proto_645_header_init(hdr, addr, PROTO_645_2007_FN_FROZE,
        PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID);
    hdr->len = sizeof(*tm);
    tm = (proto_645_07_frozen_mdhm_t*)hdr->data;
    os_mem_cpy(tm, time, sizeof(*time));
    proto_645_add33_handle(hdr->data, hdr->len);
    proto_645_tail_init(hdr);
    iot_pkt_put(pkt, PROTO_645_2007_FROZEN_DATA_PKT_LEN);
    return pkt;
}

/* create a meter reading message for the 645 protocol  */
iot_pkt_t *proto_645_build_mr_msg(uint8_t p_id,
    uint8_t *addr, uint32_t di)
{
    iot_pkt_t *pkt;
    uint8_t   fn;
    uint32_t  length;
    if (p_id != PROTO_645_1997_ID
        && p_id != PROTO_645_2007_ID)
        return NULL;
    pkt = iot_pkt_alloc(max(PROTO_645_1997_READ_DATA_PKT_LEN,
        PROTO_645_2007_READ_DATA_PKT_LEN) + PROTO_645_PREAMBLE_LEN, \
        IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    iot_pkt_reserve(pkt, PROTO_645_PREAMBLE_LEN);
    if (p_id == PROTO_645_1997_ID) {
        fn = PROTO_645_1997_FN_READ_DATA;
    } else {
        fn = PROTO_645_2007_FN_READ_DATA;
    }
    length = proto_645_fill_frame(iot_pkt_data(pkt), p_id, addr,
        PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID,
        fn, di, 0, NULL);
    IOT_ASSERT(length);
    iot_pkt_put(pkt, length);
    return pkt;
}

iot_pkt_t *proto_645_build_mr_msg_with_multi_di(uint8_t p_id,
    uint8_t *addr, uint32_t *di_buf, uint8_t cnt,
    uint8_t is_with_preamble)
{
    iot_pkt_t *pkt = NULL;
    uint8_t i, fn, *data;
    uint32_t size;
    size = PROTO_645_2007_READ_DATA_PKT_LEN;
    if (is_with_preamble) {
        size += PROTO_645_PREAMBLE_LEN;
    }
    size *= cnt;
    pkt = iot_pkt_alloc(size,IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    data = iot_pkt_data(pkt);
    for (i = 0; i < cnt; i++) {
        if (is_with_preamble) {
            os_mem_cpy(data, proto_645_preamble, PROTO_645_PREAMBLE_LEN);
            iot_pkt_put(pkt, PROTO_645_PREAMBLE_LEN);
            data += PROTO_645_PREAMBLE_LEN;
        }
        if (p_id == PROTO_645_1997_ID) {
            fn = PROTO_645_1997_FN_READ_DATA;
        } else {
            fn = PROTO_645_2007_FN_READ_DATA;
        }
        size = proto_645_fill_frame(data, p_id, addr,
            PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL,
            PROTO_645_FOLLOW_INVALID, fn, di_buf[i], 0, NULL);
        iot_pkt_put(pkt, size);
        data += size;
    }
    return pkt;
}

/* creates a nack message for the 645-2007 protocol */
iot_pkt_t *proto_645_2007_build_nack_msg(uint8_t err_code, uint8_t *addr,
    uint8_t fn)
{
    iot_pkt_t *pkt;
    proto_645_header_t *hdr;

    pkt = iot_pkt_alloc(PROTO_645_2007_NACK_PKT_LEN, IOT_SMART_GRID_MID);
    if (!pkt) {
        return NULL;
    }
    hdr = (proto_645_header_t *)iot_pkt_data(pkt);
    proto_645_header_init(hdr, addr, fn, PROTO_645_DIR_SLAVE,
        PROTO_645_ACK_ABNORMAL, PROTO_645_FOLLOW_INVALID);
    hdr->len = sizeof(err_code);
    hdr->data[0] = err_code;
    proto_645_add33_handle(hdr->data, hdr->len);
    proto_645_tail_init(hdr);
    iot_pkt_put(pkt, PROTO_645_2007_NACK_PKT_LEN);
    return pkt;
}

/* creates a ack message for the 645-2007 protocol */
iot_pkt_t *proto_645_2007_build_ack_msg(uint8_t *addr, uint8_t fn)
{
    iot_pkt_t *pkt;
    proto_645_header_t *hdr;

    pkt = iot_pkt_alloc(PROTO_645_2007_ACK_PKT_LEN, IOT_SMART_GRID_MID);
    if (!pkt) {
        return NULL;
    }
    hdr = (proto_645_header_t *)iot_pkt_data(pkt);

    iot_pkt_put(pkt, PROTO_645_2007_ACK_PKT_LEN);
    proto_645_header_init(hdr, addr, fn, PROTO_645_DIR_SLAVE,
        PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID);
    hdr->len = 0;
    proto_645_tail_init(hdr);
    return pkt;
}

/* Creates a read address message for the 645-2007 protocol */
iot_pkt_t *proto_645_2007_build_ra_msg(void)
{
    iot_pkt_t *pkt;
    uint32_t  length;
    pkt = iot_pkt_alloc(PROTO_645_2007_READ_ADDR_PKT_LEN, IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    iot_pkt_reserve(pkt,PROTO_645_PREAMBLE_LEN);
    length = proto_645_fill_frame(iot_pkt_data(pkt), PROTO_645_2007_ID,
        proto_645_any_addr, PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL,
        PROTO_645_FOLLOW_INVALID, PROTO_645_2007_FN_READ_ADDR,
        PROTO_645_INVALID_DI, 0, NULL);
    IOT_ASSERT(length);
    iot_pkt_put(pkt, length);
    return pkt;
}

uint8_t proto_645_is_bcast(uint8_t* dst)
{
    return iot_mac_addr_cmp(dst, proto_645_bcast_addr);
}

uint8_t proto_645_pm_addr_valid(uint8_t *addr)
{
    uint8_t i;
    if (!iot_mac_addr_valid(addr))
        return 0;
    if (iot_mac_addr_cmp(addr, proto_645_bcast_addr))
        return 0;
    for (i = 0; i < IOT_MAC_ADDR_LEN; i++) {
        if (addr[i] > IOT_MAC_BCD_MAX) {
            return 0;
        }
    }
    return 1;
}

uint8_t proto_645_mac_addr_cmp(const uint8_t* dst, const uint8_t* src)
{
    int8_t  i;
    uint8_t dst_wildcard_cnt = 0;
    uint8_t src_wildcard_cnt = 0;
    uint8_t cmp_cnt;
    for(i = IOT_MAC_ADDR_LEN - 1; i >= 0;) {
        if (dst[i] == PRORO_645_ANY_ADDR_BYTE) {
            i--;
            dst_wildcard_cnt++;
        } else {
            break;
        }
    }
    for(i = IOT_MAC_ADDR_LEN - 1; i >= 0;) {
        if (src[i] == PRORO_645_ANY_ADDR_BYTE) {
            i--;
            src_wildcard_cnt++;
        } else {
            break;
        }
    }
    if(dst_wildcard_cnt == IOT_MAC_ADDR_LEN ||
       src_wildcard_cnt == IOT_MAC_ADDR_LEN) {
       return 1;
    }
    cmp_cnt = IOT_MAC_ADDR_LEN - max(dst_wildcard_cnt, src_wildcard_cnt);
    return os_mem_cmp(dst, src, cmp_cnt) == 0 ? 1 : 0;
}

uint8_t proto_645_type_identify(proto_645_header_t *hdr)
{
    uint8_t proto_id = PROTO_UNKNOWN_ID;
    switch(hdr->control.fn) {
    case PROTO_645_2007_FN_READ_DATA:
    case PROTO_645_2007_FN_READ_DATA_C:
    case PROTO_645_2007_FN_READ_ADDR:
    case PROTO_645_2007_FN_WRITE_DATA:
    case PROTO_645_2007_FN_WRITE_ADDR:
    case PROTO_645_2007_FN_FROZE:
    case PROTO_645_2007_FN_CHG_RATE:
    case PROTO_645_2007_FN_CHG_PASSWORD:
    case PROTO_645_2007_FN_MAX_REQ_RESET:
    case PROTO_645_2007_FN_PM_RESET:
    case PROTO_645_2007_FN_EVENT_RESET:
    case PROTO_645_2007_FN_COST_CON:
    case PROTO_645_2007_EXT_FN_BR:
    case PROTO_645_2007_FN_BAND_SWITCH:
    case PROTO_645_2007_EXT_FN_QUERY_ID:
    {
        proto_id = PROTO_645_2007_ID;
        break;
    }
    case PROTO_645_2007_FN_SECURITY_CERTIFICATE:
    {
        /* the fn of repeating read data in 97 protocol is same as the fn of
         * security certificate in 07 protocol, but the repeated read data
         * message of the 97 protocol is not loaded with data, i.e hdr->len
         * = 0.
         */
        if (hdr->len)
            proto_id = PROTO_645_2007_ID;
        else
            proto_id = PROTO_645_1997_ID;
        break;
    }
    case PROTO_645_1997_FN_READ_DATA:
    case PROTO_645_1997_FN_READ_DATA_C:
    case PROTO_645_1997_FN_WRITE_DATA:
    case PROTO_645_1997_FN_WRITE_ADDR:
    case PROTO_645_1997_FN_CHG_RATE:
    case PROTO_645_1997_FN_CHG_PASSWORD:
    case PROTO_645_1997_FN_MAX_REQ_RESET:
    {
        proto_id = PROTO_645_1997_ID;
        break;
    }
    case PROTO_645_2007_FN_CORRECT_TIME:
    default:
    {
        /* because spec 07 and 97 has the same FN for correct time
         * and unknown fn, the protocol id is unknown;
         */
        break;
    }
    }
    return proto_id;
}

uint32_t proto_645_07_event_validate(uint8_t *data, uint8_t len)
{
    uint8_t event_status_len;

    if (len < PROTO_645_2007_EVENT_STATUS_LEN) {
        event_status_len = len;
    } else {
        event_status_len = PROTO_645_2007_EVENT_STATUS_LEN;
    }

    return iot_bitmap_cbs(data, event_status_len);
}

uint32_t proto_645_get_di_by_sub33(uint8_t *data, uint8_t len,
    uint8_t p_id, uint32_t *di)
{
    uint8_t di_t[PROTO_645_2007_DI_LEN] = {0};
    uint32_t ret = ERR_OK;

    BUILD_BUG_ON(PROTO_645_2007_DI_LEN > PROTO_645_1997_DI_LEN);
    if (p_id == PROTO_645_1997_ID) {
        if (len < PROTO_645_1997_DI_LEN) {
            ret = ERR_FAIL;
            goto out;
        }
        os_mem_cpy(di_t, data, PROTO_645_1997_DI_LEN);
        proto_645_sub33_handle(di_t, PROTO_645_1997_DI_LEN);
        *di = proto_645_1997_byte_to_di(di_t);
    } else {
        if (len < PROTO_645_2007_DI_LEN) {
            ret = ERR_FAIL;
            goto out;
        }
        os_mem_cpy(di_t, data, PROTO_645_2007_DI_LEN);
        proto_645_sub33_handle(di_t, PROTO_645_2007_DI_LEN);
        *di = proto_645_2007_byte_to_di(di_t);
    }
 out:
    return ret;
}

uint32_t proto_645_get_di(uint8_t *data, uint32_t len, uint8_t dir,
    uint32_t *di, uint8_t *fn)
{
    uint32_t result = ERR_FAIL;
    uint8_t proto_type;
    proto_645_header_t *hdr;

    if (di == NULL || fn == NULL) {
        result = ERR_INVAL;
        goto exit_label;
    }

    hdr = proto_645_format_check(data, len, dir);
    if (hdr == NULL) {
        /* invalid header, or invalid di. */
        result = ERR_INVAL;
        goto exit_label;
    }

    proto_type = proto_645_type_identify(hdr);
    if (proto_type == PROTO_UNKNOWN_ID) {
        result = ERR_INVAL;
        goto exit_label;
    }

    if (ERR_OK == proto_645_get_di_by_sub33(hdr->data, hdr->len,
        proto_type, di)) {
        *fn = hdr->control.fn;
        result = ERR_OK;
        goto exit_label;
    }

exit_label:
    return result;
}

uint32_t proto_645_check_di_fn_match(uint8_t *data1, uint32_t len1,
    uint8_t *data2, uint32_t len2)
{
    uint32_t di1;
    uint32_t di2;
    uint8_t fn1;
    uint8_t fn2;
    uint32_t result1;
    uint32_t result2;
    uint32_t result = ERR_FAIL;
    result1 = proto_645_get_di(data1, len1, PROTO_645_DIR_MASTER,
        &di1, &fn1);
    if (result1) {
        goto exit_label;
    }

    result2 = proto_645_get_di(data2, len2, PROTO_645_DIR_SLAVE,
        &di2, &fn2);
    if (result2) {
        goto exit_label;
    }

    if (di1 == di2 && fn1 == fn2) {
        result = ERR_OK;
    }

exit_label:
    return result;
}

uint8_t proto_645_pkt_check_handle(iot_pkt_t *pkt, uint8_t *dir,
    uint32_t *di)
{
    uint8_t ret = ERR_OK;
    proto_645_header_t *hdr_645;
    uint8_t fn;

    hdr_645 = proto_645_format_check(iot_pkt_data(pkt), iot_pkt_data_len(pkt),
        PROTO_645_DIR_MASTER);
    if (hdr_645 == NULL) {
        hdr_645 = proto_645_format_check(iot_pkt_data(pkt),
            iot_pkt_data_len(pkt), PROTO_645_DIR_SLAVE);
        if (hdr_645 == NULL) {
            ret = ERR_FAIL;
        } else {
            if (dir)
                *dir = PROTO_645_DIR_SLAVE;
            if (di)
                proto_645_get_di(iot_pkt_data(pkt), iot_pkt_data_len(pkt),
                    PROTO_645_DIR_SLAVE, di, &fn);
        }
    } else {
        if (dir)
            *dir = PROTO_645_DIR_MASTER;
        if (di)
            proto_645_get_di(iot_pkt_data(pkt), iot_pkt_data_len(pkt),
                PROTO_645_DIR_MASTER, di, &fn);
    }

    return ret;
}

uint8_t proto_645_is_data_read(uint8_t *data, uint16_t len)
{
    uint8_t ret = 0;
    proto_645_header_t *head;
    head = proto_645_format_check(data, len, PROTO_645_DIR_MASTER);
    if (!head) {
        goto out;
    }
    if (head->control.fn == PROTO_645_2007_FN_READ_DATA ||
        head->control.fn == PROTO_645_2007_FN_READ_DATA_C ||
        head->control.fn == PROTO_645_1997_FN_READ_DATA ||
        head->control.fn == PROTO_645_1997_FN_READ_DATA_C) {
        ret = 1;
    }
out:
    return ret;
}

uint8_t proto_645_is_password_cmd(uint8_t *data, uint16_t len)
{
    uint8_t ret = 0;
    proto_645_header_t *head;
    head = proto_645_format_check(data, len, PROTO_645_DIR_MASTER);
    if (!head) {
        goto out;
    }
    if (head->control.fn == PROTO_645_2007_FN_CHG_PASSWORD ||
        head->control.fn == PROTO_645_2007_FN_SECURITY_CERTIFICATE ||
        head->control.fn == PROTO_645_2007_FN_COST_CON ||
        head->control.fn == PROTO_645_1997_FN_CHG_PASSWORD) {
        ret = 1;
    }
out:
    return ret;
}

proto_645_header_t *proto_645_format_check_ext(uint8_t *data,
    uint32_t len, uint32_t dir)
{
    uint32_t len_t = len, d_len, i;
    uint8_t *data_s = data;
    proto_645_header_t *head;
    proto_645_tailer_t *tail;
again:
    len = len_t;
    for (i = 0x0; i < len; i++) {
        if (data_s[i] == PROTO_645_START_CHAR) {
            data_s += i;
            len_t -= i;
            break;
        }
    }
    if (i == len) {
        goto drop;
    }
    if (len_t < sizeof(proto_645_header_t)) {
        goto drop;
    }
    head = (proto_645_header_t *)data_s;
    if (head->start_char_1 != PROTO_645_START_CHAR
        || head->start_char_2 != PROTO_645_START_CHAR) {
        data_s++;
        len_t--;
        goto again;
    }
    d_len = head->len;
    if (len_t < (sizeof(proto_645_header_t) + d_len + \
        sizeof(proto_645_tailer_t))) {
        data_s++;
        len_t--;
        goto again;
    }
    tail = (proto_645_tailer_t *)(head->data + d_len);
    if (tail->end_char != PROTO_645_END_CHAR) {
        data_s++;
        len_t--;
        goto again;
    }
    if (proto_645_calc_cs(head) != tail->cs) {
        data_s++;
        len_t--;
        goto again;
    }
    if (head->control.dir != dir) {
        len_t = len_t - sizeof(*head) - sizeof(*tail) - d_len;
        if (len_t) {
            data_s = (uint8_t *)(tail + 1);
            goto again;
        }
        goto drop;
    }
    goto out;
drop:
    data_s = NULL;
out:
    return (proto_645_header_t *)data_s;
}

uint8_t proto_645_unsig_to_bcd(uint32_t v, uint8_t bcd_byte_n,
    uint8_t *bcd)
{
    uint8_t len = 0, i;
    uint32_t v_max = 0;
    uint32_t mul, div = 0;
    for (i = 0, mul = 1; i < bcd_byte_n; i++) {
        v_max +=  99 * mul;
        div = mul;
        mul *= 100;
    }
    v = v  > v_max ? v_max : v;
    while (div) {
        bcd[len++] = iot_byte_to_bcd((uint8_t)(v / div));
        v %= div;
        div /= 100;
    }
    IOT_ASSERT(bcd_byte_n == len);
    iot_data_reverse(bcd, len);
    return len;
}

int32_t proto_645_bcd_to_integer(uint8_t *bcd, uint8_t bcd_byte_n,
    uint8_t is_signed)
{
    uint8_t i, n = bcd_byte_n << 1, digital;
    int32_t v = 0;
    int32_t mul = 1;
    int32_t sig = 1;
    for (i = 0; i < n; i++) {
        if (i & 0x01) {
            digital = bcd[i >> 1] >> 4;
            if ((i >> 1) == (bcd_byte_n - 1) && is_signed) {
                if (digital & 0x8) {
                    sig = -1;
                }
                digital &= 0x7;
            }
        } else {
            digital = bcd[i >> 1] & 0xf;
        }
        if (digital > 9) {
            return 0;
        }
        v += digital * mul;
        mul *= 10;
    }
    return (v * sig);
}

uint8_t proto_645_bcd_data_check(uint8_t *bcd, uint8_t bcd_byte_n, uint8_t is_signed)
{
    uint8_t i;

    for (i = 0; i < bcd_byte_n; i++) {
        if ((bcd[i] & 0xF) > 9) {
            return 0;
        }
        if (((bcd[i] >> 4) & 0xF) > 9) {
            if (i == (bcd_byte_n - 1) && is_signed) {
                continue;
            }
            return 0;
        }
    }
    return 1;
}

int32_t proto_645_ascii_to_integer(uint8_t *ascii, uint8_t ascii_byte_n)
{
    iot_data_reverse(ascii, ascii_byte_n);
    return iot_atoi((const char*)ascii);
}

uint8_t proto_645_sig_to_bcd(int32_t v, uint8_t bcd_byte_n,
    uint8_t *bcd)
{
    uint8_t len = 0, i;
    uint32_t v_max = 0, v_abs;
    uint32_t mul, div = 0;
    for (i = 0, mul = 1; i < bcd_byte_n; i++) {
        if (i == bcd_byte_n - 1)
            v_max += 79 * mul;
        else
            v_max += 99 * mul;
        div = mul;
        mul *= 100;
    }
    v_abs = IOT_ABS(v);
    v_abs = v_abs  > v_max ? v_max : v_abs;
    while(div) {
        bcd[len++] = iot_byte_to_bcd((uint8_t)(v_abs / div));
        v_abs %= div;
        div /= 100;
    }
    IOT_ASSERT(bcd_byte_n == len);
    if (v < 0) {
        bcd[0] |= 0x80;
    }
    iot_data_reverse(bcd, len);
    return len;
}

uint8_t proto_645_rtctime_to_YYMMDDhhmm(iot_time_tm_t *tm, uint8_t *buf)
{
    buf[0] = iot_byte_to_bcd(tm->tm_min);
    buf[1] = iot_byte_to_bcd(tm->tm_hour);
    buf[2] = iot_byte_to_bcd(tm->tm_mday);
    buf[3] = iot_byte_to_bcd(tm->tm_mon);
    buf[4] = iot_byte_to_bcd((uint8_t)(tm->tm_year - 2000));
    return PROTO_645_YYMMDDHHMM_LEN;
}

uint8_t proto_645_rtctime_to_YYMMDDhhmmss(iot_time_tm_t *tm, uint8_t *buf)
{
    buf[0] = iot_byte_to_bcd(tm->tm_sec);
    buf[1] = iot_byte_to_bcd(tm->tm_min);
    buf[2] = iot_byte_to_bcd(tm->tm_hour);
    buf[3] = iot_byte_to_bcd(tm->tm_mday);
    buf[4] = iot_byte_to_bcd(tm->tm_mon);
    buf[5] = iot_byte_to_bcd((uint8_t)(tm->tm_year - 2000));
    return PROTO_645_YYMMDDHHMMSS_LEN;
}

uint8_t proto_645_07_get_curve_point_len(uint32_t di)
{
    switch(di) {
    case PROTO_645_2007_DI_CR_V_A:
    case PROTO_645_2007_DI_CR_V_B:
    case PROTO_645_2007_DI_CR_V_C:
        return PROTO_645_V_LEN;
    case PROTO_645_2007_DI_CR_V_ALL:
        return PROTO_645_V_LEN * 3;
    case PROTO_645_2007_DI_CR_I_A:
    case PROTO_645_2007_DI_CR_I_B:
    case PROTO_645_2007_DI_CR_I_C:
    case PROTO_645_2007_EXT_CR_I_A:
    case PROTO_645_2007_EXT_CR_I_B:
    case PROTO_645_2007_EXT_CR_I_C:
        return PROTO_645_07_A_LEN;
    case PROTO_645_2007_DI_CR_I_ALL:
    case PROTO_645_2007_EXT_CR_I_ALL:
        return PROTO_645_07_A_LEN * 3;
    case PROTO_645_2007_DI_CR_P_T:
    case PROTO_645_2007_DI_CR_P_A:
    case PROTO_645_2007_DI_CR_P_B:
    case PROTO_645_2007_DI_CR_P_C:
    case PROTO_645_2007_DI_CR_Q_T:
    case PROTO_645_2007_DI_CR_Q_A:
    case PROTO_645_2007_DI_CR_Q_B:
    case PROTO_645_2007_DI_CR_Q_C:
        return PROTO_645_07_P_LEN;
    case PROTO_645_2007_DI_CR_P_ALL:
    case PROTO_645_2007_DI_CR_Q_ALL:
        return PROTO_645_07_P_LEN * 4;
    case PROTO_645_2007_DI_CR_PF_T:
    case PROTO_645_2007_DI_CR_PF_A:
    case PROTO_645_2007_DI_CR_PF_B:
    case PROTO_645_2007_DI_CR_PF_C:
        return PROTO_645_07_PF_LEN;
    case PROTO_645_2007_DI_CR_PF_ALL:
        return PROTO_645_07_PF_LEN * 4;
    case PROTO_645_2007_DI_CR_EP_POS:
    case PROTO_645_2007_DI_CR_EP_NEG:
    case PROTO_645_2007_DI_CR_EQ_POS:
    case PROTO_645_2007_DI_CR_EQ_NEG:
        return PROTO_645_07_ENERGY_DATA_LEN;
    case PROTO_645_2007_DI_CR_EPEQ_ALL:
        return PROTO_645_07_ENERGY_DATA_LEN * 4;
    case PROTO_645_2007_DI_CR_EQ_QRT1:
    case PROTO_645_2007_DI_CR_EQ_QRT2:
    case PROTO_645_2007_DI_CR_EQ_QRT3:
    case PROTO_645_2007_DI_CR_EQ_QRT4:
        return PROTO_645_07_ENERGY_DATA_LEN;
    case PROTO_645_2007_DI_CR_EQ_QRT_ALL:
        return PROTO_645_07_ENERGY_DATA_LEN * 4;
    case PROTO_645_2007_EXT_CR_EP_POS_A:
    case PROTO_645_2007_EXT_CR_EP_NEG_A:
    case PROTO_645_2007_EXT_CR_EP_POS_B:
    case PROTO_645_2007_EXT_CR_EP_NEG_B:
    case PROTO_645_2007_EXT_CR_EP_POS_C:
    case PROTO_645_2007_EXT_CR_EP_NEG_C:
        return PROTO_645_07_ENERGY_DATA_LEN;
    case PROTO_645_2007_EXT_CR_TEMPERATURE:
        return PROTO_645_07_TEMP_LEN;
    case PROTO_645_2007_EXT_CR_HUMIDITY:
        return PROTO_645_EXT_HUM_LEN;
    case PROTO_645_2007_EXT_CR_EP_DATA_BLOCKS:
        return sizeof(proto_645_ext_brm_cus_data_blocks_rsp_t);
    default:
        return 0;
    }
}

iot_pkt_t *proto_645_build_band_switch_rsp(const uint8_t *addr)
{
    proto_645_header_t *hdr;
    iot_pkt_t *pkt = NULL;
    uint32_t di = PROTO_645_2007_DI_BAND_SWITCH;

    pkt = iot_pkt_alloc(PROTO_645_07_BADN_SWITCH_DATA_LEN + sizeof(*hdr) +
        sizeof(proto_645_tailer_t), IOT_SMART_GRID_MID);
    if (pkt) {
        hdr = (proto_645_header_t *)iot_pkt_data(pkt);
        /* fill in 645 header */
        proto_645_header_init(hdr, (uint8_t*)addr,
            PROTO_645_2007_FN_BAND_SWITCH, PROTO_645_DIR_SLAVE,
            PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID);
        hdr->len = PROTO_645_07_BADN_SWITCH_DATA_LEN;
        proto_645_2007_di_to_byte(di, hdr->data);
        proto_645_add33_handle(hdr->data, hdr->len);
        proto_645_tail_init(hdr);
        iot_pkt_put(pkt, PROTO_645_07_BADN_SWITCH_DATA_LEN + sizeof(*hdr) +
            sizeof(proto_645_tailer_t));
    }
    return pkt;
}

iot_pkt_t *proto_645_2007_build_mr_rsp(uint8_t *addr, uint32_t di,
    uint8_t *payload, uint8_t len, uint8_t head_reserved_len)
{
    uint8_t *data;
    iot_pkt_t *pkt = NULL;
    proto_645_header_t *hdr;

    if (!addr) {
        return pkt;
    }
    /* node addr, little-endian */
    pkt = iot_pkt_alloc(sizeof(*hdr) + sizeof(proto_645_tailer_t)
        + PROTO_645_2007_DI_LEN + len + head_reserved_len,
        IOT_SMART_GRID_MID);
    if (pkt) {
        iot_pkt_reserve(pkt, head_reserved_len);
        data = iot_pkt_data(pkt);
        hdr = (proto_645_header_t *)data;
        proto_645_header_init(hdr, addr,
            PROTO_645_2007_FN_READ_DATA, PROTO_645_DIR_SLAVE,
            PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID);
        iot_pkt_put(pkt, sizeof(*hdr));
        data += sizeof(*hdr);
        hdr->len = 0;
        if (di != PROTO_645_INVALID_DI) {
            proto_645_2007_di_to_byte(di, data);
            hdr->len += PROTO_645_2007_DI_LEN;
            data += PROTO_645_2007_DI_LEN;
        }
        if (payload && (len > 0)) {
            os_mem_cpy(data, payload, len);
            hdr->len += len;
            data += len;
        }
        proto_645_add33_handle(hdr->data, hdr->len);
        proto_645_tail_init(hdr);
        iot_pkt_put(pkt, hdr->len + sizeof(proto_645_tailer_t));
    }
    return pkt;
}

iot_pkt_t *proto_645_1997_build_mr_rsp(uint8_t *addr, uint32_t di,
    uint8_t *payload, uint8_t len, uint8_t head_reserved_len)
{
    uint8_t *data;
    iot_pkt_t *pkt = NULL;
    proto_645_header_t *hdr;

    if (!addr) {
        return pkt;
    }
    /* node addr, little-endian */
    pkt = iot_pkt_alloc(sizeof(*hdr) + sizeof(proto_645_tailer_t)
        + PROTO_645_1997_DI_LEN + len + head_reserved_len,
        IOT_SMART_GRID_MID);
    if (pkt) {
        iot_pkt_reserve(pkt, head_reserved_len);
        data = iot_pkt_data(pkt);
        hdr = (proto_645_header_t *)data;
        proto_645_header_init(hdr, addr,
            PROTO_645_1997_FN_READ_DATA, PROTO_645_DIR_SLAVE,
            PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID);
        iot_pkt_put(pkt, sizeof(*hdr));
        data += sizeof(*hdr);
        hdr->len = 0;
        if (di != PROTO_645_INVALID_DI) {
            proto_645_1997_di_to_byte(di, data);
            hdr->len += PROTO_645_1997_DI_LEN;
            data += PROTO_645_1997_DI_LEN;
        }
        if (payload && (len > 0)) {
            os_mem_cpy(data, payload, len);
            hdr->len += len;
            data += len;
        }
        proto_645_add33_handle(hdr->data, hdr->len);
        proto_645_tail_init(hdr);
        iot_pkt_put(pkt, hdr->len + sizeof(proto_645_tailer_t));
    }
    return pkt;
}


iot_pkt_t *proto_645_2007_build_w_rsp(uint8_t *addr)
{
    uint8_t *data;
    iot_pkt_t *pkt = NULL;
    proto_645_header_t *hdr;

    if (!addr) {
        return pkt;
    }
    /* node addr, little-endian */
    pkt = iot_pkt_alloc(sizeof(*hdr) + sizeof(proto_645_tailer_t),
        IOT_SMART_GRID_MID);
    if (pkt) {
        data = iot_pkt_put(pkt, sizeof(*hdr) + sizeof(proto_645_tailer_t));
        hdr = (proto_645_header_t *)data;
        proto_645_header_init(hdr, addr, PROTO_645_2007_FN_WRITE_DATA,
            PROTO_645_DIR_SLAVE, PROTO_645_ACK_NORMAL,
            PROTO_645_FOLLOW_INVALID);
        hdr->len = 0;
        proto_645_tail_init(hdr);
    }
    return pkt;
}

void proto_645_check_frame_handler(uint8_t* buffer, uint32_t buffer_len,
    bool_t* is_frame)
{
    uint8_t d_len;
    proto_645_header_t *head;
    proto_645_tailer_t *tail;
    do {
        if (buffer_len < sizeof(proto_645_header_t)) {
            *is_frame = false;
            break;
        }
        head = (proto_645_header_t*)buffer;
        if (head->start_char_1 != PROTO_645_START_CHAR
            || head->start_char_2 != PROTO_645_START_CHAR) {
            *is_frame = false;
            break;
        }
        d_len = head->len;
        if (buffer_len != (sizeof(proto_645_header_t) + d_len + \
            sizeof(proto_645_tailer_t))) {
            *is_frame = false;
            break;
        }
        tail = (proto_645_tailer_t*)(buffer + sizeof(*head) + d_len);
        if (proto_645_calc_cs(head) != tail->cs) {
            *is_frame = false;
            break;
        }
        if (tail->end_char != PROTO_645_END_CHAR) {
            *is_frame = false;
            break;
        }
        *is_frame = true;
    } while(0);
}

iot_pkt_t *proto_645_07_build_mr_lr_msg(uint8_t *addr,
    uint32_t di, uint8_t n, iot_time_tm_t *start_tm)
{
    iot_pkt_t *pkt;
    uint32_t len;
    uint8_t *data;
    proto_645_07_curve_dl_t dl = {0};
    pkt = iot_pkt_alloc(PROTO_645_PREAMBLE_LEN + sizeof(proto_645_header_t) +
        PROTO_645_2007_DI_LEN + sizeof(proto_645_tailer_t)+ sizeof(dl),
        IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    data = iot_pkt_reserve(pkt, PROTO_645_PREAMBLE_LEN);
    dl.n = iot_byte_to_bcd(n);
    dl.year = iot_byte_to_bcd((uint8_t)(start_tm->tm_year - 2000));
    dl.mon = iot_byte_to_bcd(start_tm->tm_mon);
    dl.mday = iot_byte_to_bcd(start_tm->tm_mday);
    dl.hour = iot_byte_to_bcd(start_tm->tm_hour);
    dl.min = iot_byte_to_bcd(start_tm->tm_min);
    len = proto_645_fill_frame(data, PROTO_645_2007_ID, addr,
        PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL, PROTO_645_FOLLOW_INVALID,
        PROTO_645_2007_FN_READ_DATA, di, sizeof(dl), (uint8_t *)&dl);
    iot_pkt_put(pkt, len);
    return pkt;
}

iot_pkt_t *proto_645_07_build_mr_lr_msg_with_multi_di(uint8_t *addr,
    uint32_t *di_buf, uint8_t cnt, iot_time_tm_t *start_tm, uint8_t n,
    uint8_t is_with_preamble)
{
    iot_pkt_t *pkt;
    uint32_t len;
    uint8_t *data, i;
    proto_645_07_curve_dl_t dl = {0};
    len = sizeof(proto_645_header_t) + \
        PROTO_645_2007_DI_LEN + sizeof(proto_645_tailer_t)+ sizeof(dl);
    if (is_with_preamble) {
        len += PROTO_645_PREAMBLE_LEN;
    }
    len *= cnt;
    pkt = iot_pkt_alloc(len, IOT_SMART_GRID_MID);
    if (!pkt)
        return NULL;
    data = iot_pkt_data(pkt);
    for (i = 0; i < cnt; i++) {
        if (is_with_preamble) {
            os_mem_cpy(data, proto_645_preamble, PROTO_645_PREAMBLE_LEN);
            iot_pkt_put(pkt, PROTO_645_PREAMBLE_LEN);
            data += PROTO_645_PREAMBLE_LEN;
        }
        dl.n = iot_byte_to_bcd(n);
        dl.year = iot_byte_to_bcd((uint8_t)(start_tm->tm_year - 2000));
        dl.mon = iot_byte_to_bcd(start_tm->tm_mon);
        dl.mday = iot_byte_to_bcd(start_tm->tm_mday);
        dl.hour = iot_byte_to_bcd(start_tm->tm_hour);
        dl.min = iot_byte_to_bcd(start_tm->tm_min);
        len = proto_645_fill_frame(data, PROTO_645_2007_ID, addr,
            PROTO_645_DIR_MASTER, PROTO_645_ACK_NORMAL,
            PROTO_645_FOLLOW_INVALID, PROTO_645_2007_FN_READ_DATA, di_buf[i],
            sizeof(dl), (uint8_t *)&dl);
        iot_pkt_put(pkt, len);
        data += len;
    }
    return pkt;
}

void proto_645_i_97_to_07(uint8_t *i_97, uint8_t *i_07)
{
    BUILD_BUG_ON(PROTO_645_07_A_LEN == 3);
    BUILD_BUG_ON(PROTO_645_97_A_LEN == 2);
    i_07[0] = (i_97[0] & 0x0f) << 4;
    i_07[1] = ((i_97[0] & 0xf0) >> 4) | ((i_97[1] & 0x0f) << 4);
    i_07[2] = (i_97[1] & 0xf0) >> 4;
}

void proto_645_rp_97_to_07(uint8_t *rp_97, uint8_t *rp_07)
{
    BUILD_BUG_ON(PROTO_645_07_P_LEN == 3);
    BUILD_BUG_ON(PROTO_645_97_RP_LEN == 2);
    rp_07[0] = 0x0;
    rp_07[1] = rp_97[0];
    rp_07[2] = rp_97[1];
}

uint32_t proto_645_corr_msg_check(uint8_t *data, uint32_t len, uint8_t *addr)
{
    uint32_t ret = ERR_INVAL;
    proto_645_header_t *hdr_645;

    if (!data || !len || !addr) {
        goto out;
    }
    hdr_645 = proto_645_format_check(data, len, PROTO_645_DIR_MASTER);
    if (!hdr_645) {
        goto out;
    }
    if (hdr_645->control.fn != PROTO_645_2007_FN_CORRECT_TIME) {
        goto out;
    }
    if (hdr_645->len < sizeof(proto_645_corr_time_t)) {
        goto out;
    }
    iot_mac_addr_cpy(addr, hdr_645->addr);
    ret = ERR_OK;
out:
    return ret;
}

iot_pkt_t *proto_645_build_msg(uint8_t *addr, uint8_t *data, uint8_t len,
    uint32_t di, uint8_t dir, uint8_t ack, uint8_t fn, uint8_t p_id,
    uint8_t follow)
{
    uint16_t pkt_len;
    uint32_t length;
    iot_pkt_t *pkt;

    if (p_id != PROTO_645_1997_ID && p_id != PROTO_645_2007_ID) {
        return NULL;
    }

    pkt_len = PROTO_645_PREAMBLE_LEN + sizeof(proto_645_header_t) +
        PROTO_645_2007_DI_LEN + len + sizeof(proto_645_tailer_t);
    pkt = iot_pkt_alloc(pkt_len, IOT_SMART_GRID_MID);
    if (!pkt) {
        return NULL;
    }
    iot_pkt_reserve(pkt, PROTO_645_PREAMBLE_LEN);
    length = proto_645_fill_frame(iot_pkt_data(pkt), p_id, addr, dir, ack,
        follow, fn, di, len, data);
    IOT_ASSERT(length);
    iot_pkt_put(pkt, length);
    return pkt;
}