armfly/User/app/iec104/iec104_queue.c

/**
************************************************************************************************
* @文件    : Iec10x.c
* @作者    : 樊春春
* @版本    : V1.0
* @时间    : 2022/07/09 16:18:42
* @邮箱    : [email protected]
* @说明    :
************************************************************************************************
**/

#include "iec104_queue.h"

p_iec104_t          iec104            = NULL;
iec_16u             iec104_state_addr = 0;
iec104_prio_queue_t iec10x_prio_queue_array[IEC10X_PRIO_MAX];

iec_16u      IEC10X_Cp16time2a   = 0;
iec_16u      IEC10X_Cp16time2a_V = 0;
CP56Time2a_T IEC10X_Cp56time2a;

void iec104_lock(void)
{

#ifdef IEC104LOCK
    if (iec104->lock != NULL)
        iec104->lock();
#endif
}

void iec104_unlock(void)
{

#ifdef IEC104LOCK
    if (iec104->unlock != NULL)
        iec104->unlock();
#endif
}

void iec104_queue_init(void)
{
    iec_8u i;
    for (i = 0; i < IEC10X_PRIO_MAX; i++)
    {
        iec104->queue_init(&iec10x_prio_queue_array[i]);
    }
}

void iec104_queue_clear(void)
{
    iec_8u i;
    for (i = 0; i < IEC10X_PRIO_MAX; i++)
    {
        iec104->queue_clear(&iec10x_prio_queue_array[i]);
    }
}

iec_8u iec104_prio_get(iec_8u State)
{

    /*Prio from 0~7*/
    iec_8u Prio = 0;

    return Prio;
}

void iec104_enqueue(iec_8u *enq_buf, iec_16u Length, iec_8u Prio,
                    void (*IEC10XCallBack)(iec104_callback_arg_t *Arg), iec104_callback_arg_t *CallbackArg)
{

    iec104_prio_node_t *new_p;

    iec104_lock();
    if (Length < IEC10X_HEADER_LENGTH)
    {
        LOG("IEC10X_Enqueue,buffer too short \r\n");
        goto END;
    }

    new_p = (iec104_prio_node_t *)iec104->malloc(sizeof(iec104_prio_node_t) + Length - 1);
    if (new_p == NULL)
    {
        LOG("IEC10X_Enqueue,malloc error \r\n");
        goto END;
    }

    memcpy(new_p->value, enq_buf, Length);
    new_p->Length = Length;

    /* Prio from 1~8, Array from 0~7*/
    if (Prio >= IEC10X_PRIO_MAX)
    {
        LOG("IEC104_Enqueue, error Prio(%d) \r\n", Prio);
        goto END;
    }

    /* Set callback Argument */
    new_p->CallBack = IEC10XCallBack;
    if (CallbackArg != NULL)
        new_p->CallBackArg = *CallbackArg;

    new_p->CallBackArg.value = new_p->value;
    LOG("IEC104_Enqueue,Prio(%d) elementNum(%d)len(%d)(%d) \r\n", Prio, iec10x_prio_queue_array[Prio].element_num, Length, new_p->Length);
    // DumpHEX(new_p->value, new_p->Length);
    iec104->enqueue(&iec10x_prio_queue_array[Prio], new_p);
END:
    iec104_unlock();
    return;
}

iec104_prio_node_t *iec104_dequeue(void)
{
    iec_8u              Prio;
    iec104_prio_node_t *ret;
    Prio = iec104->get_prio();
    if (Prio >= 8)
    {
        LOG("IEC104_Dequeue, Error Prio(%d) \r\n", Prio);
        return 0;
    }
    ret = iec104->dequeue(&iec10x_prio_queue_array[Prio]);
    if (ret)
    {
        iec10x_prio_queue_array[Prio].element_num--;
    }
    LOG("IEC104_Dequeue(%d) \r\n", ret->Length);
    return ret;
}
iec104_prio_node_t *IEC104_FindQHead(void)
{

    iec_8u              Prio;
    iec104_prio_node_t *ret;
    Prio = iec104->get_prio();
    if (Prio >= 8)
    {
        return 0;
    }
    ret = iec104->find_qhead(&iec10x_prio_queue_array[Prio]);
    return ret;
}

void iec104_scheduled(int socketfd)
{

    iec104_prio_node_t *de_q_node;

    de_q_node = iec104_dequeue();

    if (de_q_node)
    {

        LOG("<<-------------------IEC10X (%d)-------------------->> \r\n", DeQNode->Length);

        /* call back funtion */
        if (de_q_node->CallBack)
            de_q_node->CallBack(&(de_q_node->CallBackArg));

        dump_hex(DeQNode->value, DeQNode->Length);
        iec104->send(socketfd, (char *)(de_q_node->value), de_q_node->Length);

        iec104_lock();
        iec104->free(de_q_node);
        iec104_unlock();
    }
}

iec_32s iec104_module_register(void *p_iec10x)
{

    int ret;
    if (NULL == p_iec10x)
    {
        return -1;
    }
    else
    {
        iec104 = (p_iec104_t)p_iec10x;
        if (NULL == iec104->init)
        {
            return -1;
        }
        else
        {
            iec104_queue_init();
            // IEC10X_InitInfo();
            ret = iec104->init();
            if (0 == ret)
            {
                LOG("\r\nRegister \"%s\" IEC104 Success, < HuiXing 2014-2015 > ...\r\n", iec10x->name);
            }
            return ret;
        }
    }
}

#ifdef PRIO_QUEUE

/*******************************************************************************
* Function Name  : iec10x_prio_highest
* Description    :
* Input          : None
* Output         : None
* Return         : RET_ERROR failure
                   len sucess
*******************************************************************************/
iec_8u iec10x_prio_highest(void)
{

    int    i, prio;
    iec_8u flag = 0;
    prio        = -1;
    // LOG("%s \r\n",__FUNCTION__);
    for (i = 0; i < IEC10X_PRIO_MAX; i++)
    {
        if (iec10x_prio_queue_array[i].element_num > 0 && !flag)
        {
            // return i;
            prio = i;
            flag = 1;
            LOG("<%s>Prio[%d],Num%d. \n", __FUNCTION__, i, iec10x_prio_queue_array[i].element_num);
        }
        LOG("[%d]%d.", i, iec10x_prio_queue_array[i].element_num);
    }
    LOG("out(%d)\r\n", prio);
    return prio;
}

void iec104_prio_queue_init(iec104_prio_queue_t *PrioQueue)
{

    PrioQueue->header      = NULL;
    PrioQueue->tail        = NULL;
    PrioQueue->element_num = 0;
}
iec_8u iec104_prio_enqueue(iec104_prio_queue_t *queue_hdr, iec104_prio_node_t *new_p)
{

    new_p->Next = NULL;

    if (queue_hdr->header == NULL)
    {
        queue_hdr->header = new_p;
        queue_hdr->tail   = new_p;
    }
    else
    {
        queue_hdr->tail->Next = new_p;
        queue_hdr->tail       = new_p;
    }

    queue_hdr->element_num++;
    LOG("%s ElementNum(%d) \r\n", __FUNCTION__, queue_hdr->element_num);
    return RET_SUCESS;
}

iec104_prio_node_t *iec104_prio_dequeue(iec104_prio_queue_t *queue_hdr)
{

    iec104_prio_node_t *PrioNode_DeQ;
    if (queue_hdr->header == NULL)
    {
        LOG("PrioDeQueue,error \r\n");
        return NULL;
    }
    PrioNode_DeQ = queue_hdr->header;

    queue_hdr->header = queue_hdr->header->Next;
    if (queue_hdr->header == NULL)
    {
        queue_hdr->tail = NULL;
    }

    LOG("PrioDeQueue(%d) \r\n", PrioNode_DeQ->Length);

    return PrioNode_DeQ;
}

iec104_prio_node_t *iec104_prio_queue_find_head(iec104_prio_queue_t *queue_hdr)
{

    iec104_prio_node_t *PrioNode_DeQ;
    if (queue_hdr->header == NULL)
    {
        // LOG("PrioDeQueue,error \r\n");
        return NULL;
    }
    PrioNode_DeQ = queue_hdr->header;

    // LOG("PrioDeQueue(%d) \r\n",PrioNode_DeQ->Length);

    return PrioNode_DeQ;
}

void *IEC10X_PeekQueue(iec104_prio_queue_t *queue_hdr)
{

    if (queue_hdr->header == NULL)
    {
        // LOG("   ");
        return NULL;
    }
    return queue_hdr->header->value;
}

int IEC10X_Prio_IsEmptyQueue(iec104_prio_queue_t *queue_hdr)
{

    if (queue_hdr->header == NULL)
    {
        return 1;
    }
    else
    {
        return 0;
    }
}

void iec104_prio_queue_clear(iec104_prio_queue_t *iec10x_prio_queue)
{

    iec104_prio_node_t *p = iec10x_prio_queue->header;
    while (p != NULL)
    {
        iec10x_prio_queue->header = iec10x_prio_queue->header->Next;
        iec104->free(p);
        p = iec10x_prio_queue->header;
    }
    iec10x_prio_queue->tail        = NULL;
    iec10x_prio_queue->element_num = 0;
    return;
}

#endif