NIM_DSP6748/code/hlx/tr_queue.c

/*
 * queue.c
 *
 *  Created on: 2025年8月20日
 *      Author: 龙三郎
 */
#include <tr_queue.h>
#include <stdio.h>


// 初始化队列
void initializeQueue(Queue *q)
{
    q->front = 0;
    q->rear = 0;
}

// 判断队列是否已满
bool isFull(Queue *q)
{
    return (q->rear + 1) % QUEUE_MAX_SIZE == q->front;
}

// 判断队列是否为空
bool isEmpty(Queue *q)
{
    return q->front == q->rear;
}

// 队列现有元素个数
int currentItems(Queue *q)
{
    if(q->rear >= q->front) {
        return q->rear - q->front;
    } else {
        return q->rear + QUEUE_MAX_SIZE - q->front + 1;
    }
}

// 队列剩余空间
int remainSpace(Queue *q)
{
    return QUEUE_MAX_SIZE - currentItems(q);
}

// 入队列，单个元素
bool enqueue(Queue *q, unsigned char element)
{
    if (isFull(q)) {
        return false; // 队列已满，无法添加新元素
    }
    q->items[q->rear] = element; // 将元素添加到队尾
    q->rear = (q->rear + 1) % QUEUE_MAX_SIZE; // 更新队尾指针，注意循环队列的处理方式
    return true; // 成功入队返回true
}

// 批量入队列
bool enqueueBatch(Queue *q, unsigned char *elements, unsigned int elSize)
{
    if(remainSpace(q) < elSize)
    {
        return false; // 队列空间不够，无法批量添加。
    }
    int i;
    for(i = 0; i < elSize; i++)
    {
        enqueue(q, *(elements + i));
    }
    return true;
}

// 出队列，单个元素
bool dequeue(Queue *q, unsigned char *element)
{
    if (isEmpty(q)) {
        return false; // 队列为空，无法移除元素
    }
    *element = q->items[q->front]; // 获取队首元素的值并传出参数（可选）
    q->front = (q->front + 1) % QUEUE_MAX_SIZE; // 更新队首指针，注意循环队列的处理方式
    return true; // 成功出队返回true
}

// 批量出队列
bool dequeueBatch(Queue *q, unsigned short len, unsigned char *elements)
{
    if (currentItems(q) < len) {
        return false; // 长度不够
    }
    int i;
    for(i = 0; i < len; i++)
    {
        dequeue(q, elements + i);
    }
    return true; // 成功出队返回true
}


// 查看队列中的元素，不出队列
static bool queryQueue(Queue *q, unsigned short start, unsigned short len, unsigned char *element)
{
    if (currentItems(q) < (start + len)) {
        return false; // 长度不够
    }
    int i;
    for(i = 0; i < len; i++)
    {
        int index = (q->front + start + i) % QUEUE_MAX_SIZE;
        *(element + i) = q->items[index]; // 获取队首元素的值并传出参数（可选）
    }
    return true; // 成功出队返回true
}




// 数据转发协议相关
// 将源数据打包成协议数据
static bool packageSourceData(unsigned char *s, unsigned short sl, unsigned char *d, unsigned short *dl, unsigned char da, unsigned char sa, unsigned char type)
{
    unsigned short length = sl + 7;
    *(d) = 'T';
    *(d+1) = 'R';
    *(d+2) = 0x01;
    *(d+3) = (da & 0x0f) | ((sa << 4) & 0xf0);
    *(d+4) = (length >> 8) & 0xff;
    *(d+5) = length & 0xff;
    *(d+6) = type;
    memcpy((d+7), s, sl);
    *dl = length;
    return true;
}
// 将数据加上协议格式，并放入队列。
static unsigned char buffer[BUFFER_SIZE_2K];
static unsigned short bufferLength = 0;
bool enqueueBatchWithProtocol(Queue *q, unsigned char *s, unsigned short sl, unsigned char da, unsigned char sa, unsigned char type)
{
    packageSourceData(s, sl, buffer, &bufferLength, da, sa, type);
    return enqueueBatch(q, buffer, bufferLength);
}
// 从队列中取出一个数据包。
bool dequeueBatchWithProtocol(Queue *q, unsigned char *d, unsigned short *dl)
{
    while(true)
    {
        unsigned char pheader[3] = {0};
        if(!queryQueue(q, 0, 3, pheader))
        {
            return false;
        }
        if(pheader[0] == 'T' && pheader[1] == 'R' && pheader[2] == 0x01)
        {
            break;
        }
        else
        {
            dequeue(q, pheader);
        }
    }
    unsigned char plength[2] = {0};
    if(!queryQueue(q, 4, 2, plength))
    {
        return false;
    }
    unsigned short length = 0;
    length |= (plength[0] << 8) & 0xff00;
    length |= plength[1] & 0x00ff;
//    printf("length=%d\r\n", length);
    *dl = length;
    return dequeueBatch(q, length, d);
}





// 协议解析相关
// 获取协议版本
static bool verifyProtocol(unsigned char *p, unsigned short pl)
{
    if(pl < 7)
    {
        return false;
    }
    unsigned short length = 0;
    length |= (*(p + 4) << 8) & 0xff00;
    length |= *(p + 5) & 0x00ff;
    if(length != pl)
    {
        return false;
    }
    return true;
}
bool getVerFromProtocol(unsigned char *p, unsigned short pl, unsigned char *ver)
{
    if(!verifyProtocol(p, pl))
    {
        return false;
    }
    *ver = *(p + 2);
    return true;
}

// 获取目的地址
bool getDaFromProtocol(unsigned char *p, unsigned short pl, unsigned char *da)
{
    if(!verifyProtocol(p, pl))
    {
        return false;
    }
    *da = *(p + 3) & 0x0f;
    return true;
}

// 获取数据类型
bool getTypeFromProtocol(unsigned char *p, unsigned short pl, unsigned char *type)
{
    if(!verifyProtocol(p, pl))
    {
        return false;
    }
    *type = *(p + 6);
    return true;
}

// 获取来源地址
bool getSaFromProtocol(unsigned char *p, unsigned short pl, unsigned char *sa)
{
    if(!verifyProtocol(p, pl))
    {
        return false;
    }
    *sa = (*(p + 3) >> 4) & 0x0f;
    return true;
}

// 获取源数据
bool getPayloadFromProtocol(unsigned char *p, unsigned short pl, unsigned char *s, unsigned short *sl)
{
    if(!verifyProtocol(p, pl))
    {
        return false;
    }
    unsigned short length = pl - 7;
    *sl = length;
    memcpy(s, (p + 7), length);
    if(*sl == 0)
    {
        return false;
    }
    return true;
}




// 转发业务相关
// 初始化
Queue queueList[QUEUE_NUM];
void dataforwardInit(void)
{
    int i;
    for(i = 0; i < QUEUE_NUM; i++)
    {
        initializeQueue(&queueList[i]);
    }
}

// 主流程
static unsigned char dfmBuffer[BUFFER_SIZE_2K];
static unsigned short dfmBufferLength = 0;
void dataforwardMain(void)
{
    int i;
    for(i = 0; i < QUEUE_NUM; i++)
    {
        while(dequeueBatchWithProtocol(&queueList[i], dfmBuffer, &dfmBufferLength))
        {
            unsigned char da, sa, type;
            if(!getDaFromProtocol(dfmBuffer, dfmBufferLength, &da)
                    || !getSaFromProtocol(dfmBuffer, dfmBufferLength, &sa)
                    || !getTypeFromProtocol(dfmBuffer, dfmBufferLength, &type))
            {
                continue;
            }
            if(da == THIS_ADDRESS)
            {
                dataNotForwardedHandle(dfmBuffer, dfmBufferLength, sa, type);
            }
            else
            {
                dataForwardedHandle(dfmBuffer, dfmBufferLength, da);
            }
        }
    }

}

// 将从usb发来的协议数据入队列
bool enqueueFromUsb(unsigned char *s, unsigned short sl)
{
    return enqueueBatch(&queueList[QUEUE_INDEX_USB], s, sl);
}

// 将从以太网发来的协议数据入队列
bool enqueueFromEthernet(unsigned char *s, unsigned short sl)
{
    return enqueueBatch(&queueList[QUEUE_INDEX_ETHERNET], s, sl);
}

// 将从uart发来的协议数据入队列
bool enqueueFromUart(unsigned char c)
{
    return enqueue(&queueList[QUEUE_INDEX_UART], c);
}

// 将从uart发来的协议数据入队列
bool enqueueFromUartBatch(unsigned char *s, unsigned short sl)
{
    return enqueueBatch(&queueList[QUEUE_INDEX_UART], s, sl);
}

// 从本模块发出的数据入队列，源数据。不带协议格式
bool enqueueFromHere(unsigned char *s, unsigned short sl, unsigned char da, unsigned char type)
{
    return enqueueBatchWithProtocol(&queueList[QUEUE_INDEX_HEAR], s, sl, da, THIS_ADDRESS, type);
}