#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <ucontext.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/ioctl.h>
#include <fcntl.h>
#include <sys/mman.h>
#include "osp.h"

#include "ucp_printf.h"


typedef struct DoubleLinkNode
{
   uint64_t len;
   int32_t IsBusy;
   int32_t rsv;
   struct DoubleLinkNode *prev;
   struct DoubleLinkNode *next;
}MemNode;
#define OSP_NODE_SIZE sizeof(MemNode)
#define OSP_HEAP_BUF_TO_HEAD(pbuf) (MemNode *)((uint64_t)pbuf - OSP_NODE_SIZE)

typedef struct heap_mem
{
    MemNode *root;
    MemNode *cur_avaliable;
    lx_sem_t *heap_mutex;
    uint64_t virt_base;
    uint64_t phy_base;
    char name[24];
}Heap_Mem_CB;

typedef struct static_mem
{
    uint64_t virt_base;
    uint64_t phy_base;
    uint64_t len;
    uint32_t is_mapped;  //是否已经映射 0：未映射；1：已映射
    uint32_t rsv;
}Static_Mem_CB;


int32_t g_dev_mem_fd = -1;
int32_t g_dev_mem_noncache_fd = -1;

Static_Mem_CB g_static_mem_cb[MEM_RESERVE] = 
{
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },
    {
        .is_mapped = 0,
    },    
};

MemNode *osp_search_node(MemNode *head, uint32_t size)
{
    MemNode *node;
    node = head;
            
    while (node)
    {
        if ((node->len > (size + OSP_NODE_SIZE)) && (!node->IsBusy))
            return node;
        node = node->next;
    }
    return OSP_OK;
}

MemNode *osp_split_node(MemNode *node, uint32_t size)
{
    MemNode *new;
    MemNode *next;
    uint32_t len;
    
    if (node->len-size < OSP_NODE_SIZE)
        return node;

    len = node->len;

    node->len = size;
    new = (MemNode *)((uint64_t)node + node->len + OSP_NODE_SIZE);
    
    new->len = len - size - OSP_NODE_SIZE;
    new->IsBusy = 0;

    new->next = node->next;
    next = new->next;
    if (next)
        next->prev = new;
    new->prev = node;
    node->next = new;
    return new;
}
int32_t osp_set_node_busy(MemNode *node)
{
    node->IsBusy = 1;
    return OSP_OK;
}
int32_t osp_clear_node_busy(MemNode *node)
{
    node->IsBusy = 0;
    return OSP_OK;
}

MemNode *osp_get_next_node(MemNode *node)
{
    return node->next;
}

MemNode *osp_get_pre_node(MemNode *node)
{
    return node->prev;
}


int32_t osp_node_need_merge(MemNode *node, MemNode *nextnode)
{
    if ((0 == node->IsBusy)&&(0 == nextnode->IsBusy))
        return 1;
    return 0;
}

int32_t osp_node_merge(Heap_Mem_CB *heap_mem, MemNode *node, MemNode *nextnode)
{
    MemNode *nextnext;

    nextnext = nextnode->next;
    
    node->next = nextnext;
    if (nextnext)
        nextnext->prev = node;

    node->len += nextnode->len + OSP_NODE_SIZE;
 
    if(nextnode == heap_mem->cur_avaliable)
    {
        heap_mem->cur_avaliable = node;
    }

    return OSP_OK;

}

static int32_t get_base_and_len(module_type_e       module, uint64_t *base_addr,uint64_t *len)
{
    int32_t ret = OSP_OK;
    
    switch(module)
    {
        case ARM_STACK:
        {
            *base_addr = ARM_STACK_BASE_PHY_ADDR;
            *len = LEN_OF_ARM_STACK;
            break;
        }
        case APE_DM:
        {
            *base_addr = APE_DM_BASE_PHY_ADDR;
            *len = LEN_OF_APE_DM;
            break;
        }        
        case APE_PHY:
        {
            *base_addr = APE_PHY_BASE_PHY_ADDR;
            *len = LEN_OF_APE_PHY;
            break;
        }
        case APE_TEXT:
        {
            *base_addr = APE_TEXT_BASE_PHY_ADDR;
            *len = LEN_OF_APE_TEXT;
            break;
        }
        case ARM_APE_MSG:
        {
            *base_addr = ARM_APE_MSG_BASE_PHY_ADDR;
            *len = LEN_OF_ARM_APE_MSG;
            break;
        }
        /*case ARM_LOG:
        {
            *base_addr = ARM_LOG_BASE_PHY_ADDR;
            *len = LEN_OF_ARM_LOG;
            break;
        }*/ 
        case APE_LOG:
        {
            *base_addr = APE_LOG_BASE_PHY_ADDR;
            *len = LEN_OF_APE_LOG;
            break;
        }
        case SHARE_MEM:
        {
            *base_addr = SHARE_MEM_BASE_PHY_ADDR;
            *len = LEN_OF_SHARE_MEM;
            break;
        }
        case ECS_SM:
        {
            *base_addr = ECS_SM_DM_BASE_PHY_ADDR;
            *len = LEN_OF_ECS_SM_DM;
            break;
        }
        case PET_SM:
        {
            *base_addr = PET_SM_DM_BASE_PHY_ADDR;
            *len = LEN_OF_PET_SM_DM;
            break;
        }        
        default:
        {
            *base_addr = 0;
            *len = 0;
            ret = OSP_ERROR;
            break;
        }
    }

    return ret;
}

void *get_static_mem(module_type_e      module, uint64_t *len)
{
    void *mem_vir_addr;
    uint64_t addr_offset = 0,mem_len = 0;
    int32_t mem_temp_fd = -1;
    
    if(1 == g_static_mem_cb[module].is_mapped) //已映射  
    {
        *len = g_static_mem_cb[module].len;
        return (void *)g_static_mem_cb[module].virt_base;
    }
    

//#ifdef CACHE_ENABLE    
    switch(module)
    {
        case ARM_STACK:
#ifdef CACHE_ENABLE
        case ARM_APE_MSG:   
#endif        
        {
            if(OSP_ERROR == g_dev_mem_fd) //打开mem设备
            {
                g_dev_mem_fd = open("/dev/ucp_sm_ddr_cache", O_RDWR|O_SYNC);
            }
            
            if(g_dev_mem_fd < 0)
            {
                UCP_PRINT_ERROR("open file failed,ucp_sm_ddr_cache < 0.");
                return (void *)OSP_ERROR;
            } 
            mem_temp_fd = g_dev_mem_fd;
            break;
        } 
#ifndef CACHE_ENABLE
        case ARM_APE_MSG:
#endif        
        case ECS_SM:
        case PET_SM:
        case APE_DM:
        case SHARE_MEM:
        case APE_PHY:
        case APE_TEXT:
        case APE_LOG:
        {
            //g_dev_mem_fd = open("/dev/ucp_2_revmem", O_RDWR|O_SYNC);
            if(OSP_ERROR == g_dev_mem_noncache_fd) //打开mem设备
            {
                g_dev_mem_noncache_fd = open("/dev/ucp_sm_ddr_noncache", O_RDWR|O_SYNC);
            }

            if(g_dev_mem_noncache_fd < 0)
            {
                UCP_PRINT_ERROR("open file failed,ucp_sm_ddr_noncache < 0.");
                return (void *)OSP_ERROR;
            }
            mem_temp_fd = g_dev_mem_noncache_fd;
            break;
        }
        default: 
            UCP_PRINT_ERROR("memory type error. < 0.");
            return (void *)OSP_ERROR; 
    }    
/*#else
    if(OSP_ERROR == g_dev_mem_fd) //打开mem设备
    {
        //g_dev_mem_fd = open("/dev/mem", O_RDWR|O_SYNC);
        g_dev_mem_fd = open("/dev/ucp_2_revmem", O_RDWR|O_SYNC);
        if(g_dev_mem_fd < 0)
        {
            return (void *)OSP_ERROR;
        }
    }
    mem_temp_fd = g_dev_mem_fd;
#endif*/
    /*通过模块类型获取对应的地址偏移和长度,并进行地址映射*/ 
    if(OSP_OK == get_base_and_len(module, &addr_offset, &mem_len)) 
    {
        mem_vir_addr = mmap(NULL, mem_len, PROT_READ|PROT_WRITE, MAP_SHARED, mem_temp_fd, addr_offset);
        if((void *)OSP_ERROR == mem_vir_addr)
        {
            UCP_PRINT_ERROR("get_base_and_len failed,%lx,%lx.",addr_offset,mem_len);
            return (void *)OSP_ERROR;
        }
        else
        {
            *len = mem_len;
            g_static_mem_cb[module].virt_base = (uint64_t)mem_vir_addr;
            g_static_mem_cb[module].phy_base = addr_offset;
            g_static_mem_cb[module].len = mem_len;
            g_static_mem_cb[module].is_mapped = 1;
            return mem_vir_addr;
        }
    }
    else
    {
        UCP_PRINT_ERROR("get_base_and_len error,%lx,%lx.",addr_offset,mem_len);
        return (void *)OSP_ERROR;
    }
}

void *osp_heap_mem_init(char *pbuf, uint64_t size,void *head_of_static_mem,module_type_e module)
{
    Heap_Mem_CB *heap_mem = NULL;
    uint64_t addr_offset = 0,mem_len = 0;

    heap_mem = malloc(sizeof(Heap_Mem_CB));
    if(NULL == heap_mem)
    {
        return NULL;
    }
    
    heap_mem->root = (MemNode *)pbuf;
    if(size < OSP_NODE_SIZE)  //长度小于结构头长度
    {
        heap_mem->root->len = 0;
    }
    else
    {
        heap_mem->root->len = size - OSP_NODE_SIZE;
    }
    
    heap_mem->root->IsBusy = 0;
    heap_mem->root->next = NULL;
    heap_mem->root->prev = NULL;
    heap_mem->virt_base = (uint64_t)head_of_static_mem;
    heap_mem->cur_avaliable = heap_mem->root;

    if(OSP_OK == get_base_and_len(module, &addr_offset, &mem_len))
    {
        heap_mem->phy_base = addr_offset;
        heap_mem->heap_mutex = osp_semm_create();
        return (void *)heap_mem;
    }
    else
    {
        return NULL;
    }
}

int8_t * osp_alloc_heap_mem(void *heap, uint32_t size)
{
    MemNode *node;
    //MemNode *nextnode;
    //MemNode *new;
    Heap_Mem_CB *heap_mem = (Heap_Mem_CB *)heap;
    
    osp_sem_take(heap_mem->heap_mutex, -1);
    
    node = osp_search_node(heap_mem->cur_avaliable, size);
    if (!node)
    {
        node = osp_search_node(heap_mem->root, size);
        if (!node)
        {
            osp_sem_give(heap_mem->heap_mutex);
            return NULL;
        }
    }
  
    //osp_set_node_busy(node);
    node->IsBusy = 1;

    //new = osp_split_node(node, size);
    heap_mem->cur_avaliable = osp_split_node(node, size);
        
        
    //nextnode = osp_get_next_node(new);
    /*nextnode = new->next;
    if (NULL != nextnode)
        if (osp_node_need_merge(new, nextnode))
            osp_node_merge(new, nextnode);*/
    
    osp_sem_give(heap_mem->heap_mutex);
    return (int8_t *)((uint64_t)node + OSP_NODE_SIZE);

}

int32_t osp_free_heap_mem(void *heap, char*pbuf)
{
    MemNode *node = OSP_HEAP_BUF_TO_HEAD(pbuf);
    MemNode *nextnode;
    MemNode *prenode;
    Heap_Mem_CB *heap_mem = (Heap_Mem_CB *)heap;
    
    osp_sem_take(heap_mem->heap_mutex, -1);
    //osp_clear_node_busy(node);
    node->IsBusy = 0;
    
    //osp_debug_out(CMD_DEBUG_LEVEL, "node is %p, node->IsBusy is %d, node->len is %d\r\n", node, node->IsBusy, node->len);
    //nextnode = osp_get_next_node(node);
    nextnode = node->next;
    
    //prenode = osp_get_pre_node(node);
    prenode = node->prev;
    
    if (nextnode)
    {
        if (osp_node_need_merge(node, nextnode))
        {
            osp_node_merge(heap_mem, node, nextnode);
        }
    }

    if (prenode)
    {
        if (osp_node_need_merge(prenode, node))
        {
            osp_node_merge(heap_mem, prenode, node);
        }
    }
    osp_sem_give(heap_mem->heap_mutex);
    
    return 0;
}

int32_t osp_show_heap_mem(void *heap)
{
    int32_t busynum = 0;
    int32_t freenum = 0;
    int32_t busymem = 0;
    int32_t freemem = 0;
    char buf[256] = {0};
    int32_t len = 0;
    Heap_Mem_CB *heap_mem = (Heap_Mem_CB *)heap;
    
    MemNode *node = heap_mem->root;
    osp_sem_take(heap_mem->heap_mutex, -1);
    while (node)
    {
        osp_debug_out(CMD_DEBUG_LEVEL, "node is %p, node->IsBusy is %d, node->len is %d\r\n", node, node->IsBusy, node->len);
        if (node->IsBusy)
        {
            busynum++;
            busymem += node->len;
        }
        else
        {
            freenum++;
            freemem += node->len;            
        }
        node = node->next;
    }
    
    osp_sem_give(heap_mem->heap_mutex);
    len += sprintf(buf + len, "busynum\tbusymem\tfreenum\tfreemem\t\r\n");
    len += sprintf(buf + len, "%d\t%d\t%d\t%d\t\r\n", busynum, busymem, freenum, freemem);
    osp_debug_out(CMD_DEBUG_LEVEL, "%s", buf);

    return OSP_OK;
}

#if 0
U64 osp_phy_to_virt(void *heap, U64 phy_addr)
{
    Heap_Mem_CB *heap_mem = (Heap_Mem_CB *)heap;

    return (heap_mem->virt_base + (phy_addr - heap_mem->phy_base));
}

U64 osp_virt_to_phy(void *heap, U64 virt_addr)
{
    Heap_Mem_CB *heap_mem = (Heap_Mem_CB *)heap;

    return (heap_mem->phy_base + (virt_addr - heap_mem->virt_base));
}
#else
int32_t osp_phy_to_virt(module_type_e       module, uint64_t phy_addr, uint64_t *virt_addr)
{
    if(1 == g_static_mem_cb[module].is_mapped) 
    {
        *virt_addr = g_static_mem_cb[module].virt_base + (phy_addr - g_static_mem_cb[module].phy_base);
        return OSP_OK;
    }
    else
    {
        UCP_PRINT_ERROR("osp_phy_to_virt %d error!!",module);
        return OSP_ERROR;
    }
}

int32_t osp_virt_to_phy(module_type_e       module, uint64_t virt_addr, uint64_t *phy_addr)
{
    if(1 == g_static_mem_cb[module].is_mapped)  
    {
        *phy_addr = g_static_mem_cb[module].phy_base + (virt_addr - g_static_mem_cb[module].virt_base);
        return OSP_OK;
    }
    else
    {
        UCP_PRINT_ERROR("osp_virt_to_phy %d error!!",module);    
        return OSP_ERROR;
    }
}
#endif

#if 1
void  osp_flush_dcache_area(void * volatile addr, uint64_t volatile len)
{
    
__asm volatile("     mrs     x3, ctr_el0 \n"                   // read CTR
                    "     ubfm   x3, x3, #16, #19  \n"            // cache line size encoding
                    "     mov     x2, #4    \n"                    // bytes per word
                    "     lsl     x2, x2, x3    \n"      // actual cache line size
                    "     add     x1, x0, x1   \n"
                    "     sub     x3, x2, #1   \n"
                    "     bic     x0, x0, x3     \n"
                    " 9998:                         \n"
                    "     dc      civac,  x0      \n"
                    "     add      x0, x0, x2      \n"
                    "     cmp     x0, x1        \n"
                    "     b.lo    9998b             \n"
                    "     dsb     sy        \n");
    
}

void  osp_clean_dcache_area(void * volatile addr, uint64_t volatile len)
{

__asm volatile("     mrs     x3, ctr_el0 \n"                   // read CTR
                    "     ubfm   x3, x3, #16, #19  \n"            // cache line size encoding
                    "     mov     x2, #4    \n"                    // bytes per word
                    "     lsl     x2, x2, x3    \n"      // actual cache line size
                    "     add     x1, x0, x1   \n"
                    "     sub     x3, x2, #1   \n"
                    "     bic     x0, x0, x3     \n"
                    " 9998:                         \n"
                    "     dc      cvac,  x0      \n"
                    "     add      x0, x0, x2      \n"
                    "     cmp     x0, x1        \n"
                    "     b.lo    9998b             \n"
                    "     dsb     sy        \n");
    
}
#endif

int32_t osp_invalid_dcache_area(module_type_e          module, uint64_t virt_addr, uint64_t len)
{
    int32_t iRet = 0;
    uint64_t phy_addr = 0;

    if(g_dev_mem_fd < 0)
    {
        return OSP_ERROR;
    }

    if(OSP_OK != osp_virt_to_phy(module, virt_addr, &phy_addr))
    {
        return OSP_ERROR;
    }

    iRet = ioctl(g_dev_mem_fd,len,phy_addr);
    if(-1 == iRet)
    {
        printf("user_invalid_dcache_area phy add out of range \n ");
        return OSP_ERROR;
    }

    return OSP_OK;
}

int32_t osp_display_phy_mem(uint64_t phy_addr, uint32_t len)
{
    uint64_t addr_temp = OSP_ROUND_DOWN(phy_addr,OSP_DISPLAY_PAGE_LEN);
    uint64_t addr_offset = phy_addr - addr_temp;
    uint32_t *mem_vir_addr,*temp_vir_addr; 
    uint32_t i,j;
    int32_t display_fd = 1;
    
    if(OSP_DISPLAY_MAX_LEN < len)
    {
       return OSP_ERROR; 
    }

    //printf("phy_addr=%#llx, mmap_addr=%#llx,virt_addr=%#llx,len = %d:\n",phy_addr,addr_temp,len); 
    printf("phy_addr=%#lx, mmap_addr=%#lx,len = %d\n",phy_addr,addr_temp,len);
    

    display_fd = open("/dev/mem", O_RDWR|O_SYNC);
    if(display_fd < 0)
    {
        printf("open /dev/mem error\n");
        return OSP_ERROR;
    }

    
    mem_vir_addr = (uint32_t*)mmap(NULL, (OSP_DISPLAY_PAGE_LEN << 1), PROT_READ|PROT_WRITE, MAP_SHARED, display_fd, addr_temp);
    if((uint32_t*)OSP_ERROR == mem_vir_addr)
    {
        printf("mmap error!! return value is %ld\n",(uint64_t)mem_vir_addr);
        return OSP_ERROR;
    }
    
    temp_vir_addr = mem_vir_addr + (addr_offset >> 2);

    for(i = 0; i < len; i += 16)
    {//不足16字节也显示4个字
        printf("0x%016lx: ",phy_addr + i);
        
        for(j = 0; j < 4; j++)
        {
            printf("0x%08x  ",*temp_vir_addr++);
        }

        printf("\n");
    }
    
    munmap((void*)mem_vir_addr,(OSP_DISPLAY_PAGE_LEN << 1));
    close(display_fd);
    return OSP_OK;
}

