// +FHDR------------------------------------------------------------
//                 Copyright (c) 2022 SmartLogic.
//                       ALL RIGHTS RESERVED
// -----------------------------------------------------------------
// Filename      : msg_queue_common.c
// Author        : xianfeng.du
// Created On    : 2022-06-25
// Last Modified :
// -----------------------------------------------------------------
// Description:
//
//
// -FHDR------------------------------------------------------------

#include <stdatomic.h>
#include <assert.h>
#include <stddef.h>
#include <string.h>
#include "ospHeap.h"
#include "mem_sections.h"
#include "ucp_printf.h"
#include "ucp_utility.h"
#include "pet_sm_mgt.h"
#include "msg_transfer_mem.h"
#include "msg_transfer_mbuffer.h"

static inline void* get_mbuf_ptr(uint8_t* base, uint32_t size, uint16_t idx)
{
    void *mbuf = (void *)(base + (size * idx));
    return mbuf;
}

static inline void msg_queue_dl_setup(uint32_t instId, uint32_t qNo, uint8_t* bufBase, uint32_t bufSize, uint32_t numOfBufs)
{
    assert(isPowerOf2(numOfBufs));

    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t *)&pMsgQueueLocalMgt->localDlQueue[instId][qNo];
    ch->bufBase = bufBase;
    uint64_t phy_addr;
    osp_virt_to_phy(ARM_APE_MSG, (uint64_t)bufBase, (uint64_t *)&phy_addr);

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pDlQueue[instId][qNo];
    memset((void *)chCommon, 0, sizeof(MsgQueueCommonInfo_t));
    chCommon->ringSize = (uint16_t)numOfBufs;
    chCommon->ringMask = (uint16_t)(numOfBufs - 1);
    chCommon->bufBase = (uint32_t)phy_addr;
    chCommon->bufSize = bufSize;

    uint8_t *m = bufBase;
    MsgMemBufAttr_t *mbuf;
    for (uint32_t k = 0; k < numOfBufs; k++)
    {
        mbuf = (MsgMemBufAttr_t *)m;
        mbuf->qNo = (uint8_t)qNo;
        mbuf->state = 1;
        mbuf->idx = k;
        m += bufSize;
    }

    UCP_PRINT_LOG("Setup DL Queue[0x%08x] ch = 0x%lx", qNo, (uint64_t)chCommon);
    UCP_PRINT_LOG("qNo = 0x%08x bufBase = 0x%lx bufSize = 0x%08x numOfBufs = 0x%08x",qNo, (uint64_t)bufBase, bufSize, numOfBufs);

    return;
}

int32_t msg_queue_dl_block_init(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t*)&pMsgQueueLocalMgt->localDlQueue[inst_id][que_id];

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pDlQueue[inst_id][que_id];

    ch->availableSize = chCommon->bufSize - MSG_MBUF_HEAD_SIZE;
    ch->offset = 0;
    ch->msgCnt = 0;

    return SUCCESS;
}

int32_t msg_queue_dl_alloc_buf(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t*)&pMsgQueueLocalMgt->localDlQueue[inst_id][que_id];

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pDlQueue[inst_id][que_id];

#if 0
    ucp_spin_lock(&lock_dl_tx_alloc);
    ch->idx = chCommon->alloc & chCommon->ringMask;
    chCommon->alloc++;
    ucp_spin_unlock(&lock_dl_tx_alloc);
#else
    ch->idx = atomic_fetch_add(&chCommon->alloc,1) & chCommon->ringMask;
#endif

    MsgMemBufAttr_t *mbuf = (MsgMemBufAttr_t *)get_mbuf_ptr(ch->bufBase, chCommon->bufSize, ch->idx);
    ch->bufAddr = (uint64_t)&mbuf[1];

    UCP_PRINT_LOG("msg_queue_dl_alloc_buf,buf:0x%lx,availableSize:0x%08x,offset:0x%08x", ch->bufAddr, ch->availableSize, ch->offset);

    return SUCCESS;
}

int32_t msg_queue_dl_put_buf(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t*)&pMsgQueueLocalMgt->localDlQueue[inst_id][que_id];
    if (ch->msgCnt == 0)
    {
        //ch->offset
        return SUCCESS;
    }

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pDlQueue[inst_id][que_id];

    MsgMemBufAttr_t *mbuf = (MsgMemBufAttr_t *)get_mbuf_ptr(ch->bufBase, chCommon->bufSize, ch->idx);
    mbuf->msgSize = ch->offset;

#if 0
    ucp_spin_lock(&lock_dl_tx_put);
    chCommon->in++;
    ucp_spin_unlock(&lock_dl_tx_put);
#else
    atomic_fetch_add(&chCommon->in,1);
#endif

    UCP_PRINT_LOG("msg_queue_dl_put_buf[%d] chCommon info alloc[%d],in[%d],out[%d]",que_id,chCommon->alloc,chCommon->in,chCommon->out);

    return SUCCESS;
}

static inline void msg_queue_dl_free(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t *)&pMsgQueueLocalMgt->localDlQueue[inst_id][que_id];

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pDlQueue[inst_id][que_id];

    memset((void *)ch, 0, sizeof(MsgQueueLocalInfo_t));
    memset((void *)chCommon, 0, sizeof(MsgQueueCommonInfo_t));

    return;
}

static inline void msg_queue_ul_setup(uint32_t instId, uint32_t qNo, uint8_t* bufBase, uint32_t bufSize, uint32_t numOfBufs)
{
    assert(isPowerOf2(numOfBufs));

    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t *)&pMsgQueueLocalMgt->localUlQueue[instId][qNo];
    ch->bufBase = bufBase;
    uint64_t phy_addr;
    osp_virt_to_phy(ARM_APE_MSG, (uint64_t)bufBase, (uint64_t *)&phy_addr);

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pUlQueue[instId][qNo];
    memset((void *)chCommon, 0, sizeof(MsgQueueCommonInfo_t));
    chCommon->ringSize = (uint16_t)numOfBufs;
    chCommon->ringMask = (uint16_t)(numOfBufs - 1);
    chCommon->bufBase = (uint32_t)phy_addr;
    chCommon->bufSize = bufSize;

    osp_virt_to_phy(ECS_SM, (uint64_t)ch->bufIdxBase, (uint64_t *)&phy_addr);
    chCommon->bufIdxBase = (uint32_t)phy_addr;

    uint8_t *m = bufBase;
    MsgMemBufAttr_t *mbuf;
    for (uint32_t k = 0; k < numOfBufs; k++)
    {
        mbuf = (MsgMemBufAttr_t *)m;
        mbuf->qNo = (uint8_t)qNo;
        mbuf->state = 1;
        mbuf->idx = k;
        m += bufSize;
    }

    UCP_PRINT_LOG("Setup UL Queue[0x%08x] ch = 0x%lx", qNo, (uint64_t)chCommon);
    UCP_PRINT_LOG("qNo = 0x%08x bufBase = 0x%lx bufSize = 0x%08x numOfBufs = 0x%08x",qNo, (uint64_t)bufBase, bufSize, numOfBufs);

    return;
}


uint8_t* msg_queue_ul_get_buf(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t*)&pMsgQueueLocalMgt->localUlQueue[inst_id][que_id];
    uint16_t* pIdxBase = (uint16_t *)ch->bufIdxBase;

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pUlQueue[inst_id][que_id];

    uint32_t avail = chCommon->in - chCommon->out;
    if (0 == avail)
    {
        return NULL;
    }

    uint16_t idx = chCommon->out & chCommon->ringMask;
    uint16_t bufIdx = pIdxBase[idx];

    MsgMemBufAttr_t *mbuf = (MsgMemBufAttr_t *)get_mbuf_ptr(ch->bufBase, chCommon->bufSize, bufIdx);
    uint8_t *buf = (uint8_t *)&mbuf[1];
    chCommon->out++;

    return buf;
}

static inline void msg_queue_ul_free(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueLocalInfo_t* ch = (MsgQueueLocalInfo_t *)&pMsgQueueLocalMgt->localUlQueue[inst_id][que_id];

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCommonInfo_t* chCommon = pPetSmLocalMgt->pUlQueue[inst_id][que_id];

    memset((void *)ch, 0, sizeof(MsgQueueLocalInfo_t));
    memset((void *)chCommon, 0, sizeof(MsgQueueCommonInfo_t));

    return;
}

void msg_transfer_queue_setup(uint8_t inst_id, uint8_t que_id)
{
    MEM_SECTION_INFO* pMemSection = GetMsgDdrSection();
    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();

    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueCfg_t* pQueueCfg = (MsgQueueCfg_t *)&pMsgQueueLocalMgt->localQueueCfg[inst_id][que_id];
    memset((void *)&pMsgQueueLocalMgt->localDlQueue[inst_id][que_id],0,sizeof(MsgQueueLocalInfo_t));
    memset((void *)&pMsgQueueLocalMgt->localUlQueue[inst_id][que_id],0,sizeof(MsgQueueLocalInfo_t));

    uint16_t block_num;
    uint32_t block_size;
    uint8_t* ptr;

    block_num = pQueueCfg->tx_queue.block_num;
    block_size = pQueueCfg->tx_queue.block_size;
    ptr = (uint8_t *)memSectionAlloc(pMemSection, block_num*block_size, MEM_ALIGNED_4BYTES, "dlAddr");
    msg_queue_dl_setup(inst_id, que_id, ptr, block_size, block_num);

    block_num = pQueueCfg->rx_queue.block_num;
    block_size = pQueueCfg->rx_queue.block_size;
    memset((void *)pPetSmLocalMgt->pBufIdxBase, 0, (block_num*sizeof(uint16_t)));
    pMsgQueueLocalMgt->localUlQueue[inst_id][que_id].bufIdxBase = (uint8_t *)pPetSmLocalMgt->pBufIdxBase;
    pPetSmLocalMgt->pBufIdxBase += block_num;
    ptr = (uint8_t *)memSectionAlloc(pMemSection, block_num*block_size, MEM_ALIGNED_4BYTES, "ulAddr");
    msg_queue_ul_setup(inst_id, que_id, (uint8_t *)ptr, block_size, block_num);

    return;
}


void msg_transfer_queue_free(uint8_t inst_id, uint8_t que_id)
{
    MsgQueueLocalMgt_t* pMsgQueueLocalMgt = get_msg_queue_local_mgt();
    MsgQueueCfg_t* pLocalQueueCfg = (MsgQueueCfg_t *)&pMsgQueueLocalMgt->localQueueCfg[inst_id][que_id];

    PetSmLocalMgt_t* pPetSmLocalMgt = get_pet_sm_local_mgt();
    MsgQueueCfg_t* pCommonQueueCfg = pPetSmLocalMgt->pQueueCfg[inst_id][que_id];

    memset((void *)pLocalQueueCfg, 0, sizeof(MsgQueueCfg_t));
    memset((void *)pCommonQueueCfg, 0, sizeof(MsgQueueCfg_t));

    msg_queue_dl_free(inst_id,que_id);
    msg_queue_ul_free(inst_id,que_id);

    return;
}


