kunlun/app/smart_grid/protocol/proto_dlms.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.

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

/* os ship includes */
#include "os_mem_api.h"

/* common includes */
#include "iot_config_api.h"
#include "iot_errno_api.h"
#include "iot_ntoh_api.h"

/* protocol includes */
#include "proto_crc16.h"
#include "proto_dlms.h"

/* smart grid internal header files */
#include "iot_sg_fr.h"

uint32_t proto_dlms_hdlc_addr_parse(uint8_t *data,
    proto_dlms_hdlc_addr_t *addr)
{
    uint32_t ret = ERR_OK;
    uint8_t *addr_ptr = data;
    uint8_t addr_len = PROTO_DLMS_HDLC_ADD_MAX_LEN;
    uint8_t i;
    os_mem_set(addr, 0x0, sizeof(*addr));
    /* if address include upper address and lower address
     * addr_len is PROTO_DLMS_HDLC_ADD_MAX_LEN * 2.
     */
    addr_len = addr_len << 1;
    for (i = 0; i < addr_len; i++) {
        if (addr_ptr[i] & 0x01) {
            break;
        }
    }
    addr->len = i + 1;
    addr->type = addr->len;
    switch (addr->type) {
    case PROTO_DLMS_HDLC_ADD_TYPE_1:
    {
        addr->u_addr = (addr_ptr[0] >> 1);
        break;
    }
    case PROTO_DLMS_HDLC_ADD_TYPE_2:
    {
        addr->u_addr = (addr_ptr[0] >> 1);
        addr->l_addr = (addr_ptr[1] >> 1);
        break;
    }
    case PROTO_DLMS_HDLC_ADD_TYPE_4:
    {
        addr->u_addr = (addr_ptr[0] >> 1);
        addr->u_addr <<= 7;
        addr->u_addr |= (addr_ptr[1] >> 1);
        addr->l_addr = (addr_ptr[2] >> 1);
        addr->l_addr <<= 7;
        addr->l_addr |= (addr_ptr[3] >> 1);
        break;
    }
    default:
        goto drop;
    }
    goto out;
drop:
    ret = ERR_FAIL;
out:
    return ret;
}

void proto_dlms_hdlc_type_4_addr_to_bcd_addr(uint8_t *addr,
    proto_dlms_hdlc_addr_t *hdlc_addr)
{
    uint16_t l_addr = hdlc_addr->l_addr;
    os_mem_set(addr, 0x0, IOT_MAC_ADDR_LEN);
    addr[2] = iot_byte_to_bcd((uint8_t)(l_addr / 10000));
    l_addr %= 10000;
    addr[1] = iot_byte_to_bcd((uint8_t)(l_addr / 100));
    l_addr %= 100;
    addr[0] = iot_byte_to_bcd((uint8_t)l_addr);
}

proto_dlms_hdlc_head_t *proto_dlms_hdlc_parse(uint8_t* data,
    uint16_t len, proto_dlms_hdlc_cache_t *dlms_info)
{
    uint16_t len_t, head_len, i = 0x0;
    uint16_t rest_len;
    uint8_t  *data_s = NULL;
    uint16_t min_data_len;
    proto_dlms_hdlc_cache_t cache = {0};
    union proto_dlms_hdlc_format frame_format = {0};
    proto_dlms_hdlc_head_t* head;

    for (i = 0x0, len_t = len; i < len; i++) {
        if (data[i] == PROTO_DLMS_HDLC_START_BYTE) {
            data_s = data + i;
            break;
        }
        len_t--;
    }
    min_data_len = sizeof(proto_dlms_hdlc_head_t)
        + sizeof(proto_dlms_hdlc_tailer_t);
    if (len_t <= min_data_len) {
        goto drop;
    }
    /* start and end byte check */
    if (data_s[0] != PROTO_DLMS_HDLC_START_BYTE
        || data_s[len_t - 1] != PROTO_DLMS_HDLC_END_BYTE) {
        goto drop;
    }
    /* length check */
    head = (proto_dlms_hdlc_head_t*)data_s;
    frame_format.value = iot_ntohs(head->frame_format.value);
    if ((frame_format.bit.len + PROTO_DLMS_HDLC_START_END_LEN) != len_t) {
        goto drop;
    }

    /* dlms frame type check */
    if (frame_format.bit.frame_type != PROTO_DLMS_HDLC_FRAME_HDLC) {
        goto drop;
    }

    /* dlms frame fcs check */
    if (proto_fcs16_check(data_s + 1,
        (len_t - PROTO_DLMS_HDLC_START_END_LEN))) {
        goto drop;
    }
    rest_len = len_t - min_data_len;

    /* destination address parse */
    if (proto_dlms_hdlc_addr_parse(head->sa_and_ca, &cache.dest_addr)) {
        goto drop;
    }
    if(rest_len < cache.dest_addr.len) {
        goto drop;
    }
    rest_len -= cache.dest_addr.len;

    /* source address parse */
    if (proto_dlms_hdlc_addr_parse(head->sa_and_ca + cache.dest_addr.len,
        &cache.source_addr)) {
        goto drop;
    }
    if (rest_len < cache.source_addr.len) {
       goto drop;
    }
    rest_len -= cache.source_addr.len;

    /* get control code */
    if (rest_len < PROTO_DLMS_HDLC_CTRL_LEN) {
        goto drop;
    }
    data_s = data_s + sizeof(proto_dlms_hdlc_head_t)
        + cache.dest_addr.len + cache.source_addr.len;
    cache.control = *data_s;
    rest_len -= PROTO_DLMS_HDLC_CTRL_LEN;

    /* check if the data field and hcs exists  */
    if (rest_len == 0) {
        /* If the data field and hcs don't exist,
         * the analysis is complete.
         */
        cache.data_ptr = NULL;
        cache.data_len = 0;
        goto out;
    } else if (rest_len <= PROTO_DLMS_HDLC_CHECKSUM_LEN) {
        goto drop;
    }
    /* dlms frame hcs check */
    data_s = (uint8_t*)head;
    head_len = sizeof(proto_dlms_hdlc_head_t) + cache.dest_addr.len
        + cache.source_addr.len + PROTO_DLMS_HDLC_CTRL_LEN
        + PROTO_DLMS_HDLC_CHECKSUM_LEN;
    if (proto_fcs16_check(data_s + 1, head_len - 1)) {
        goto drop;
    }
    rest_len -= PROTO_DLMS_HDLC_CHECKSUM_LEN;

    /* get data */
    cache.data_ptr = data_s + head_len;
    cache.data_len = rest_len;
    goto out;
drop:
    head = NULL;
out:
    if ((dlms_info != NULL) && (head != NULL)) {
        *dlms_info = cache;
    }
    return head;   /* checksum success */
}