NIM_DSP6748/code/hlx/tr_protocol.c

/*
 * tr_protocol.c
 *
 *  Created on: 2025年10月13日
 *      Author: Administrator
 */
#include <stdio.h>
#include <stdint.h>
#include <string.h>
#include "tr_protocol.h"
#include "tr_tools_util.h"


static void insert(uint8_t* dest, uint16_t dest_start, uint8_t* source, uint16_t source_start, uint16_t len)
{
    uint16_t i = 0;
    while (i < len)
    {
        dest[dest_start + i] = source[source_start + i];
        i++;
    }
}


// 拼包的方法
void ProtocolInit(trProtocol* protocol) {
    protocol->version = 1;
    protocol->length = 8;
    protocol->currentOptinNumber = 0;
    protocol->optionsNum = 0;
    memset(protocol->message, 0, sizeof(protocol->message));
    protocol->message[0] = 'T';
    protocol->message[1] = 'R';
    protocol->message[2] = protocol->version;
}
// 设置目的地址
void ProtocolSetDestAddress(trProtocol* protocol, uint8_t destAddress) {
    protocol->destAddress = destAddress;
    protocol->message[3] = destAddress;
}
// 设置源地址
void ProtocolSetSourceAddress(trProtocol* protocol, uint8_t sourceAddress) {
    protocol->sourceAddress = sourceAddress;
    protocol->message[4] = sourceAddress;
}
// 设置类型
void ProtocolSetType(trProtocol* protocol, uint8_t type) {
    protocol->type = type;
    protocol->message[5] = type;
}
// 设置option
void ProtocolSetOption(trProtocol* protocol, uint16_t optionNumber, uint16_t optionLength, uint8_t* optionValue) {
    uint16_t pos_start = protocol->length, opt_length = 1;
    int delta = optionNumber - protocol->currentOptinNumber;

    if (delta < 0) {
        return;
    }
    else if (delta < 13) {
        protocol->message[pos_start] |= (delta & 0xFF) << 4;
    }
    else if (delta < 255 + 13) {
        protocol->message[pos_start] |= (0x0D & 0xFF) << 4;
        uint8_t ext = delta - 0x0D;
        protocol->message[pos_start + opt_length++] = ext;
    }
    else if (delta < 65535 + 255 + 14) {
        protocol->message[pos_start] |= (0x0E & 0xFF) << 4;
        uint16_t ext = (delta - 0x0E - 0xFF);
        protocol->message[pos_start + opt_length++] = (ext >> 8) & 0xFF;
        protocol->message[pos_start + opt_length++] = ext & 0xFF;
    }
    else {
        return;
    }
    // length
    if (optionLength < 1) {
        return;
    }
    else if (optionLength < 13) {
        protocol->message[pos_start] |= (optionLength & 0xFF);
    }
    else if (optionLength < 255 + 13) {
        protocol->message[pos_start] |= (0x0D & 0xFF);
        uint8_t ext = optionLength - 0x0D;
        protocol->message[pos_start + opt_length++] = ext;
    }
    else if (optionLength < 65535 + 255 + 14) {
        protocol->message[pos_start] |= (0x0E & 0xFF);
        uint16_t ext = (optionLength - 0x0E - 0xFF);
        protocol->message[pos_start + opt_length++] = (ext >> 8) & 0xFF;
        protocol->message[pos_start + opt_length++] = ext & 0xFF;
    }
    else {
        return;
    }

    insert(protocol->message, pos_start + opt_length, optionValue, 0, optionLength);
    opt_length += optionLength;
    protocol->length += opt_length;
    protocol->currentOptinNumber = optionNumber;
}
// 设置字符串类型的option
void ProtocolSetOptionString(trProtocol* protocol, uint16_t optionNumber, char* optionValue) {
    uint16_t len = strlen(optionValue);
    ProtocolSetOption(protocol, optionNumber, len, (uint8_t*)optionValue);
}
// 设置short类型的option
void ProtocolSetOptionShort(trProtocol* protocol, uint16_t optionNumber, uint16_t optionValue) {
    uint8_t value[2];
    value[0] = (optionValue >> 8) & 0xFF;
    value[1] = optionValue & 0xFF;
    ProtocolSetOption(protocol, optionNumber, sizeof(value), value);
}
// 设置char类型的option
void ProtocolSetOptionChar(trProtocol* protocol, uint16_t optionNumber, uint8_t optionValue) {
    uint8_t value[1];
    value[0] = optionValue & 0xFF;
    ProtocolSetOption(protocol, optionNumber, sizeof(value), value);
}
// 设置payload
void ProtocolSetPayload(trProtocol* protocol, uint16_t payloadLength, uint8_t* payload) {
    uint16_t pos = protocol->length;
    protocol->message[pos] = 0xFF;
    insert(protocol->message, pos + 1, payload, 0, payloadLength);
    protocol->length += payloadLength + 1;
}
// 设置字符串类型的payload
void ProtocolSetPayloadString(trProtocol* protocol, const char* payloadString) {
    uint16_t len = strlen(payloadString);
    ProtocolSetPayload(protocol, len, (uint8_t*)payloadString);
}
// 打包
void ProtocolPackage(trProtocol* protocol) {
    protocol->message[6] = (protocol->length + 2) >> 8 & 0xFF;
    protocol->message[7] = (protocol->length + 2) & 0xFF;
    uint16_t crc = CalCRC16(protocol->message, protocol->length);
    protocol->message[protocol->length++] = crc >> 8 & 0xFF;
    protocol->message[protocol->length++] = crc & 0xFF;
}


// 报文解析
uint8_t ProtocolCheckCRC(uint8_t* message, uint16_t messageLength) {
//    uint16_t crc = CalCRC16(message, messageLength - 2);
//    if ((crc >> 8 & 0xFF) != message[messageLength - 2]) {
//        return 0;
//    }
//    if ((crc & 0xFF) != message[messageLength - 1]) {
//        return 0;
//    }
    return 1;
}
// 获取版本号
uint8_t ProtocolGetVersion(uint8_t* message, uint16_t messageLength) {
    if (!ProtocolCheckCRC(message, messageLength)) {
        return 0;
    }
    return message[2];
}
// 获取报文类型
uint8_t ProtocolGetType(uint8_t* message, uint16_t messageLength) {
    if (!ProtocolCheckCRC(message, messageLength)) {
        return 0;
    }
    return message[5];
}
// 获取目的地址
uint8_t ProtocolGetDestAddress(uint8_t* message, uint16_t messageLength) {
    if (!ProtocolCheckCRC(message, messageLength)) {
        return 0;
    }
    return message[3];
}
// 获取源地址
uint8_t ProtocolGetSourceAddress(uint8_t* message, uint16_t messageLength) {
    if (!ProtocolCheckCRC(message, messageLength)) {
        return 0;
    }
    return message[4];
}

// option结构体
typedef struct
{
    uint8_t valid; // 是否合法
    uint16_t number; // 编号
    uint16_t length; // 长度
    uint16_t pos; // 位置
    uint8_t delta_ext; // 编号扩展，字节
    uint8_t length_ext; // 长度扩展，字节
}
trOptionStruct;

static trOptionStruct _GetOption(uint8_t* message, uint16_t pos, uint16_t number)
{
    trOptionStruct option_struct = {1, number, message[pos] & 0x0F, pos, 0, 0};
    uint8_t delta = (message[pos] & 0xF0) >> 4;
    if (delta < 0x0D)
    {
        option_struct.delta_ext = 0;
        option_struct.number += delta;
    }
    else if (delta == 0x0D)
    {
        option_struct.delta_ext = 1;
        uint8_t num = message[pos + 1];
        option_struct.number += (0x0D + num);
    }
    else if (delta == 0x0E)
    {
        option_struct.delta_ext = 2;
        uint16_t num = 0;
        num = (num | message[pos + 1]) << 8;
        num = num | message[pos + 2];
        option_struct.number += (0x0E + 0xFF + num);
    }
    else
    {
        option_struct.valid = 0;
        return option_struct;
    }

    if (option_struct.length < 0x0D)
    {
        option_struct.length_ext = 0;
    }
    else if (option_struct.length == 0x0D)
    {
        option_struct.length_ext = 1;
        uint8_t num = message[pos + option_struct.delta_ext + 1];
        option_struct.length = (0x0D + num);
    }
    else if (option_struct.length == 0x0E)
    {
        option_struct.length_ext = 2;
        uint16_t num = 0;
        num = (num | message[pos + option_struct.delta_ext + 1]) << 8;
        num = num | message[pos + option_struct.delta_ext + 2];
        option_struct.length = (0x0E + 0xFF + num);
    }
    else
    {
        option_struct.valid = 0;
        return option_struct;
    }
    return option_struct;
}

// 获取option
static trOptionStruct getOption(uint8_t* message, uint16_t messageLength, uint16_t number)
{
    uint16_t pos = 8;
    uint16_t current_number = 0;
    trOptionStruct option_struct = {0};

    while (pos < messageLength)
    {
        option_struct = _GetOption(message, pos, current_number);
        if (option_struct.valid == 0 || option_struct.number == number)
        {
            break;
        }
        else
        {
            current_number = option_struct.number;
            pos = option_struct.pos + option_struct.delta_ext + option_struct.length_ext + option_struct.length + 1; // 下一个循环的位置起点
        }
    }
    return option_struct;
}

// 获取option和长度
uint16_t ProtocolGetOption(uint8_t* message, uint16_t messageLength, uint16_t optionNumber, uint8_t* optionValue) {

    trOptionStruct option_struct = getOption(message, messageLength, optionNumber);
    if (option_struct.valid)
    {
        uint8_t i;
        for (i = 0; i < option_struct.length; i++)
        {
            optionValue[i] = message[option_struct.pos + option_struct.delta_ext + option_struct.length_ext + 1 + i];
        }
        return option_struct.length;
    }
    else
    {
        return 0;
    }
}

// 获取short类型的option
uint8_t ProtocolGetOptionShort(uint8_t* message, uint16_t messageLength, uint16_t optionNumber, uint16_t * optionValue) {
    uint8_t value[8];
    uint16_t optionLength = ProtocolGetOption(message, messageLength, optionNumber, value);
    if (optionLength != 2) {
        return 0;
    }
    *optionValue = 0;
    *optionValue |= value[0] << 8;
    *optionValue |= value[1];
    return 1;
}

// 获取char类型的option
uint8_t ProtocolGetOptionChar(uint8_t* message, uint16_t messageLength, uint16_t optionNumber, uint8_t* optionValue) {
    uint8_t value[8];
    uint16_t optionLength = ProtocolGetOption(message, messageLength, optionNumber, value);
    if (optionLength != 1) {
        return 0;
    }
    *optionValue = 0;
    *optionValue |= value[0];
    return 1;
}

typedef struct
{
    uint8_t valid;
    uint16_t pos;
    uint16_t length;
}
trPayloadStruct;
// 获取payload
static trPayloadStruct Get_Payload(uint8_t* coap_message, uint16_t coap_length)
{
    uint16_t pos = 8;
    uint16_t current_number = 0;
    trOptionStruct option_struct;
    trPayloadStruct payload_struct = {1, 0, 0};

    while (pos < coap_length)
    {
        if (coap_message[pos] == 0xFF)
        {
            payload_struct.valid = 1;
            payload_struct.pos = pos + 1;
            payload_struct.length = coap_length - pos - 1;
            break;
        }
        option_struct = _GetOption(coap_message, pos, current_number);
        if (option_struct.valid == 0)
        {
            payload_struct.valid = 0;
            payload_struct.length = 0;
            payload_struct.pos = 0;
            break;
        }
        else
        {
            current_number = option_struct.number;
            pos = option_struct.pos + option_struct.delta_ext + option_struct.length_ext + option_struct.length + 1; // 下一个循环的位置起点
        }
    }
    return payload_struct;
}
// 获取payload和长度
uint16_t ProtocolGetPayload(uint8_t* message, uint16_t messageLength, uint8_t* payload) {
    if (!ProtocolCheckCRC(message, messageLength)) {
        return 0;
    }
    trPayloadStruct payload_struct = Get_Payload(message, messageLength - 2);
    if (payload_struct.valid)
    {
        uint8_t i;
        for (i = 0; i < payload_struct.length; i++)
        {
            payload[i] = message[payload_struct.pos + i];
        }
        return payload_struct.length;
    }
    else
    {
        return 0;
    }
}