/*
<:copyright-BRCM:2016:DUAL/GPL:standard

   Broadcom Proprietary and Confidential.(c) 2016 Broadcom
   All Rights Reserved

Unless you and Broadcom execute a separate written software license
agreement governing use of this software, this software is licensed
to you under the terms of the GNU General Public License version 2
(the "GPL"), available at http://www.broadcom.com/licenses/GPLv2.php,
with the following added to such license:

   As a special exception, the copyright holders of this software give
   you permission to link this software with independent modules, and
   to copy and distribute the resulting executable under terms of your
   choice, provided that you also meet, for each linked independent
   module, the terms and conditions of the license of that module.
   An independent module is a module which is not derived from this
   software.  The special exception does not apply to any modifications
   of the software.

Not withstanding the above, under no circumstances may you combine
this software in any way with any other Broadcom software provided
under a license other than the GPL, without Broadcom's express prior
written consent.

:>
 */

#include "bcmos_system.h"
#include <vxWorks.h>
#include <unistd.h>
#include <taskLib.h>
#include <wdLib.h>
#include <sysLib.h>
#include <errnoLib.h>
#include <time.h>
#include <intLib.h>
#include <cacheLib.h>
#include "memPartLib.h"

#ifdef HOST_CPU_IS_MIPS
#include "arch/mips/archMips.h"
#endif

#include <iv.h>

#define BCMOS_DEFAULT_STACK_SIZE (100*1024)/*0x4000*/
#define BCMOS_MICROSECONDS_TO_TICKS(u) (u)/1000000*bcmos_timer_get_frequency() + BCMOS_DIVIDE_ROUND_UP(((u)%1000000)*bcmos_timer_get_frequency(),1000000)

/* task control blocks */
extern STAILQ_HEAD(task_list, bcmos_task) task_list;

/* global OS lock */
extern bcmos_mutex bcmos_res_lock;

/*
 * Default task handler
 */
extern int bcmos_dft_task_handler(long data);

bcmos_errno bcmos_sys_init(void)
{
    return BCM_ERR_OK;
}

/* Clean-up system library */
void bcmos_sys_exit(void)
{
}

static uint32_t bcmos_timer_get_frequency()
{
    static uint32_t frequency = 0;
    if (!frequency)
        frequency = sysClkRateGet();
    return frequency;
}

bcmos_errno bcmos_task_create(bcmos_task *task, const bcmos_task_parm *parm)
{
    F_bcmos_task_handler handler;
    void *data;

    if (!task || !parm)
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "task %p, parm %p\n", (void* )task, (void* )parm);
    memset(task, 0, sizeof ( *task ));

    if (parm->handler)
    {
        /* "traditional task */
        handler = parm->handler;
        data = (void *)parm->data;
    }
    else
    {
        bcmos_errno rc;
        /* "integrated" task */
        handler = bcmos_dft_task_handler;
        data = task;

        /* Initialize and lock mutex to wait on */
        rc = bcmos_sem_create(&task->active_sem, 0, task->parm.flags, parm->name);
        if (rc)
        {
            BCMOS_TRACE_ERR("Task %s: can't create active_sem. Error %s (%d)\n",
                parm->name, bcmos_strerror(rc), rc);
            return rc;
        }
    }

    task->parm = *parm;
    /* Copy name to make sure that it is not released - in case it was on the stack */
    if (task->parm.name)
    {
        strncpy(task->name, task->parm.name, sizeof(task->name) - 1);
        task->parm.name = task->name;
    }
    bcmos_fastlock_init(&task->active_lock, 0);
    bcmos_mutex_lock(&bcmos_res_lock);
    STAILQ_INSERT_TAIL(&task_list, task, list);
    bcmos_mutex_unlock(&bcmos_res_lock);
    task->magic = BCMOS_TASK_MAGIC;
    task->sys_task.t = taskSpawn((char*)parm->name,
                        10 + parm->priority * 2,
                        0,
                        parm->stack_size ? parm->stack_size : BCMOS_DEFAULT_STACK_SIZE,
                        (FUNCPTR)handler,
                        (int32_t)data,
                        0,
                        0,
                        0,
                        0,
                        0,
                        0,
                        0,
                        0,
                        0);

    if (task->sys_task.t == ERROR)
    {
        bcmos_mutex_lock(&bcmos_res_lock);
        STAILQ_REMOVE(&task_list, task, bcmos_task, list);
        bcmos_mutex_unlock(&bcmos_res_lock);
        task->magic = 0;
        if (!parm->handler)
        {
            bcmos_sem_destroy(&task->active_sem);
        }
        BCMOS_TRACE_ERR("(%s) Error: taskSpawn, errnoGet()=%d\n", __FUNCTION__, errnoGet());
        BUG();
    }

    return BCM_ERR_OK;
}

bcmos_errno bcmos_task_destroy(bcmos_task *task)
{
    if (task->magic != BCMOS_TASK_MAGIC)
    {
        return BCM_ERR_PARM;
    }
    task->destroy_request = BCMOS_TRUE;
    bcmos_mutex_lock(&bcmos_res_lock);
    STAILQ_REMOVE(&task_list, task, bcmos_task, list);
    bcmos_mutex_unlock(&bcmos_res_lock);
    task->magic = BCMOS_TASK_MAGIC_DESTROYED;
    /* The task may be waiting on semaphore. Kick it */
    if (!task->parm.handler)
    {
        bcmos_sem_post(&task->active_sem);
    }
    if (taskDelete(task->sys_task.t) != OK)
    {
        BCMOS_TRACE_ERR("(%s) Error: taskDelete, errnoGet()=%d\n", __FUNCTION__, errnoGet());
        BUG();
    }

    return BCM_ERR_OK;
}

bcmos_task *bcmos_task_current(void)
{
    uint32_t pt = taskIdSelf();
    bcmos_task *t, *tmp;

    STAILQ_FOREACH_SAFE(t, &task_list, list, tmp)
    {
        if (pt == t->sys_task.t)
            break;
    }
    return t;
}

void *bcmos_alloc(uint32_t size)
{
    return malloc(size);
}

void bcmos_free(void *ptr)
{
    free(ptr);
}

inline static void bcmos_sem_mutex_destroy(uint32_t sm)
{
    if (semDelete((SEM_ID)sm) != OK)
    {
        BCMOS_TRACE_ERR("(%s) Error: semDelete, errnoGet()=%d\n", __FUNCTION__, errnoGet());
        BUG();
    }
}

inline static bcmos_errno bcmos_sem_mutex_wait(uint32_t sm, uint32_t timeout)
{
    int errno;
    STATUS status;

    timeout = ((timeout == BCMOS_WAIT_FOREVER )? WAIT_FOREVER : BCMOS_MICROSECONDS_TO_TICKS(timeout));
    status = semTake((SEM_ID)sm, timeout);
    if (status != OK)
    {
        errno = errnoGet();
        if (errno == S_objLib_OBJ_TIMEOUT)
        {
            return BCM_ERR_TIMEOUT;
        }
        BUG();
        return BCM_ERR_INTERNAL;
    }
    return BCM_ERR_OK;
}

inline static void bcmos_sem_mutex_post(uint32_t sm)
{
    STATUS status;

    status = semGive((SEM_ID)sm);
    if (status != OK)
    {
        BCMOS_TRACE_ERR("(%s) Error: semGive, status=%d\n", __FUNCTION__, status);
        BUG();
    }
}

bcmos_errno bcmos_sem_create(bcmos_sem *sem, uint32_t count, uint32_t flags, const char *name)
{
    SEM_ID semaphore;

    semaphore = semCCreate(SEM_Q_PRIORITY, (SEM_B_STATE)count);
    if (semaphore == NULL)
    {
        BCMOS_TRACE_ERR("(%s) Error: semCCreate, errnoGet()=%d\n", __FUNCTION__, errnoGet());
        BUG();
    }
    sem->s = (uint32_t)semaphore;

    return BCM_ERR_OK;
}

void bcmos_sem_destroy(bcmos_sem *sem)
{
    bcmos_sem_mutex_destroy(sem->s);
}

bcmos_errno bcmos_sem_wait(bcmos_sem *sem, uint32_t timeout)
{
    return bcmos_sem_mutex_wait(sem->s, timeout);
}

void bcmos_sem_post(bcmos_sem *sem)
{
    bcmos_sem_mutex_post(sem->s);
}

bcmos_errno bcmos_mutex_create(bcmos_mutex *mutex, uint32_t flags, const char *name)
{
    SEM_ID semaphore;
    int options;

    /* SEM_INVERSION_SAFE: Enable (by default) the prevention of priority inversion problem */
    options = SEM_Q_PRIORITY | SEM_INVERSION_SAFE;

    semaphore = semMCreate(options);
    if (semaphore == NULL)
    {
        BCMOS_TRACE_ERR("(%s) Error: semCCreate, errnoGet()=%d\n", __FUNCTION__, errnoGet());
        BUG();
    }

    mutex->m = (uint32_t)semaphore;

    return BCM_ERR_OK;
}

void bcmos_mutex_destroy(bcmos_mutex *mutex)
{
    bcmos_sem_mutex_destroy(mutex->m);
}

void bcmos_mutex_lock(bcmos_mutex *mutex)
{
    bcmos_sem_mutex_wait(mutex->m, BCMOS_WAIT_FOREVER);
}

void bcmos_mutex_unlock(bcmos_mutex *mutex)
{
    bcmos_sem_mutex_post(mutex->m);
}

void bcmos_fastlock_init(bcmos_fastlock *lock, uint32_t flags)
{
}

long bcmos_fastlock_lock(bcmos_fastlock *lock)
{
    return (long)intLock();
}

void bcmos_fastlock_unlock(bcmos_fastlock *lock, long flags)
{
    intUnlock(flags);
}

bcmos_errno bcmos_sys_timer_create(bcmos_sys_timer *timer, bcmos_sys_timer_handler handler, void *data)
{
    WDOG_ID watchdog;

    if (!timer || !handler)
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "timer %p, handler %p\n", timer, handler);

    /* Call the underlying OS */
    watchdog = wdCreate();
    if (watchdog == NULL)
        BCMOS_TRACE_RETURN(BCM_ERR_SYSCALL_ERR, "errno %s\n", strerror(errno));

    timer->t = (uint32_t)watchdog;
    timer->handler = handler;
    timer->data = data;

    return BCM_ERR_OK;
}

void bcmos_sys_timer_destroy(bcmos_sys_timer *timer)
{
    STATUS status;
    status = wdDelete((WDOG_ID)timer->t);
    BCMOS_TRACE_DEBUG("wdDelete: status=%d\n", status);
}

static void _bcmos_time_handler_wrapper(long data)
{
    bcmos_sys_timer * timer = (bcmos_sys_timer*)data ;
    timer->handler(timer->data);
}

void bcmos_sys_timer_start(bcmos_sys_timer *timer, uint32_t delay)
{
    STATUS status;
    status = wdStart((WDOG_ID)timer->t, BCMOS_MICROSECONDS_TO_TICKS(delay), (FUNCPTR)_bcmos_time_handler_wrapper, (long)timer);
    if (status == ERROR)
    {
        BCMOS_TRACE_ERR("wdStart status %d\n", status);
        BUG();
    }
}

void bcmos_sys_timer_stop(bcmos_sys_timer *timer)
{
    STATUS status;
    status = wdCancel((WDOG_ID)timer->t);
    if (status == ERROR)
    {
        BCMOS_TRACE_ERR("wdCancel status=%d\n", status);
        BUG();
    }
}

void bcmos_usleep(uint32_t us)
{
    struct timespec ts;

    ts.tv_sec = us / 1000000;
    ts.tv_nsec = ( us % 1000000 ) * 1000;

    nanosleep(&ts, 0);
}

uint32_t bcmos_timestamp(void)
{
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    return (ts.tv_sec * 1000000 + ts.tv_nsec / 1000);
}

/** Get current timestamp - 64 bit
 * \returns the current system timestamp (us)
 */
uint64_t bcmos_timestamp64(void)
{
    struct timespec ts;
    clock_gettime(CLOCK_REALTIME, &ts);
    return ((uint64_t)ts.tv_sec * 1000000LL + (uint64_t)ts.tv_nsec / 1000LL);
}

/*
 * Byte memory pool
 */

/* Memory block header */
typedef struct bcmos_byte_memblk
{
    bcmos_byte_pool *pool;      /** pool that owns the block */
    uint32_t size;              /** block size (bytes) including bcmos_byte_memblk header */
#ifdef BCMOS_MEM_DEBUG
    uint32_t magic;             /** magic number */
#define BCMOS_MEM_MAGIC_ALLOC   (('m'<<24) | ('b' << 16) | ('l' << 8) | 'k')
#define BCMOS_MEM_MAGIC_FREE    (('m'<<24) | ('b' << 16) | ('l' << 8) | '~')
#endif
} bcmos_byte_memblk;

/* Create byte memory pool */
bcmos_errno bcmos_byte_pool_create(bcmos_byte_pool *pool, const bcmos_byte_pool_parm *parm)
{
    if (!pool || !parm)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "pool %p, parm %p\n", pool, parm);
    }

    BCM_MEMZERO_STRUCT(pool);
    pool->parm = *parm;
    if (!pool->parm.size)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_PARM, "size %u\n", parm->size);
    }
#ifdef BCMOS_MEM_DEBUG
    pool->magic = BCMOS_BYTE_POOL_VALID;
#endif
    return BCM_ERR_OK;
}

/* Destroy memory pool */
bcmos_errno bcmos_byte_pool_destroy(bcmos_byte_pool *pool)
{
    if (pool->allocated)
    {
        BCMOS_TRACE_RETURN(BCM_ERR_STATE, "%u bytes of memory are still allocated from the pool %s\n",
            pool->allocated, pool->parm.name);
    }
#ifdef BCMOS_MEM_DEBUG
    pool->magic = BCMOS_BYTE_POOL_DELETED;
#endif
    return BCM_ERR_OK;
}

/* Allocate memory from memory pool */
void *bcmos_byte_pool_alloc(bcmos_byte_pool *pool, uint32_t size)
{
    bcmos_byte_memblk *blk;
    uint32_t byte_size;
    void *ptr;

#ifdef BCMOS_MEM_DEBUG
    BUG_ON(pool->magic != BCMOS_BYTE_POOL_VALID);
#endif

    if (size + pool->allocated > pool->parm.size)
        return NULL;

    byte_size = size + sizeof(bcmos_byte_memblk);
#ifdef BCMOS_MEM_DEBUG
    byte_size += sizeof(uint32_t); /* block suffix */
#endif
    /* ToDo: Maintain LL of allocated blocks */
    blk = (bcmos_byte_memblk *)malloc(byte_size);
    if (!blk)
        return NULL;
    ptr = (void *)(blk + 1);
    blk->size = byte_size;
    pool->allocated += byte_size;
    blk->pool = pool;
#ifdef BCMOS_MEM_DEBUG
    blk->magic = BCMOS_MEM_MAGIC_ALLOC;
    *(uint32_t *)((long)blk + byte_size - sizeof(uint32_t)) = BCMOS_MEM_MAGIC_ALLOC;
#endif

    return ptr;
}

/* Release memory allocated using bcmos_byte_pool_alloc() */
void bcmos_byte_pool_free(void *ptr)
{
    bcmos_byte_memblk *blk;
    bcmos_byte_pool *pool;

    BUG_ON(!ptr);
    blk = (bcmos_byte_memblk *)((long)ptr - sizeof(bcmos_byte_memblk));
    pool = blk->pool;
#ifdef BCMOS_MEM_DEBUG
    BUG_ON(pool->magic != BCMOS_BYTE_POOL_VALID);
    BUG_ON(blk->magic != BCMOS_MEM_MAGIC_ALLOC);
    BUG_ON(*(uint32_t *)((long)blk + blk->size - sizeof(uint32_t)) != BCMOS_MEM_MAGIC_ALLOC);
    blk->magic = BCMOS_MEM_MAGIC_FREE;
#endif
    pool->allocated -= blk->size;
    free(blk);
}

void _bcmos_backtrace(void)
{
    /*todo implement this*/
}

int ffsll(long long int i)
{
    int l = ffs(i & 0xFFFFFFFF);
    int h = ffs((i >> 32) & 0xFFFFFFFF);

    if (l)
        return l;

    if (h)
        return h + 32;

    return 0;
}

/* stub for int enable */
void bcmos_int_enable(int irq)
{
}

typedef struct bcmos_int_data
{
   int (*isr)(int irq, void *priv);
   int irq;
   void *priv;
} bcmos_int_data;

static void _bcmos_int_handler(long priv)
{
     bcmos_int_data *data = (bcmos_int_data *)priv;
     data->isr(data->irq, data->priv);
}

int bcmos_int_connect(int irq, int cpu, int flags, int (*isr)(int irq, void *priv), const char *name, void *priv)
{
   bcmos_int_data *data = bcmos_calloc(sizeof(bcmos_int_data));
   if (!data)
      return (int)BCM_ERR_NOMEM;
   data->isr = isr;
   data->irq = irq;
   data->priv = priv;
   return pciIntConnect(INUM_TO_IVEC(irq), _bcmos_int_handler, (long)data);
}

/* Disconnect system interrupt */
void bcmos_int_disconnect(int irq, void *priv)
{
    pciIntDisconnect(INUM_TO_IVEC(irq), _bcmos_int_handler);
}

/**************************************************************
 * Check in-irq status                                        *
 * related header file: "intLib.h"                            *
 * param[in]   none                                           *
 * param[out]  none                                           *
 * return      BCMOS_TRUE  (1) - in interrupt context;        *
 *             BCMOS_FALSE (0) - in task context;             *
 **************************************************************/

bcmos_bool is_irq_mode(void)
{
    return intContext();
}

// from bsp
extern int isIntDisabled();
/**************************************************************
 * Check if interrupts are disabled                           *
 * related header file: "mipsinc.h"                           *
 * param[in]   none                                           *
 * param[out]  none                                           *
 * return      BCMOS_TRUE (1) -  Gloal interrupt disable      *
               BCMOS_FALSE (0) - Gloal interrupt enable;      *
 * comment     if BSP team can provide similar function,      *
               we will call it;                               *
 * Another option is share this macro used on switch product  *
 *************************************************************/
bcmos_bool is_irq_disabled(void)
{
    return isIntDisabled();
}

/**************************************************************
 * write barrier                                              *
 * related header file: none                                  *
 * param[in]   none                                           *
 * param[out]  none                                           *
 * return none                                                *
 * comment:use "sync" base on MIPS (GNU style)                *
 *************************************************************/

void bcmos_barrier(void)
{
 #if defined(HOST_CPU_IS_MIPS) || defined(HOST_CPU_IS_PPC)
    {__asm__ __volatile__("sync");}
 #else
   #error HOST CPU MUST BE MIPS OR POWERPC
 #endif
}

/**************************************************************
 * Allocate DMA-able memory                                   *
 * related header file:memPartLib.h                           *
 * param[in]   device  Device id                              *
 * param[in]   size    Block size (bytes)                     *
 * param[out]  none                                           *
 * returns    memory block pointer(uncached memory) or NULL   *
 * comment:   BSP need to allocate uncached memory pool first *
 * if BSP team can provide such function,call it directly;    *
 * suggestion: memPartCreate()create uncached memory and      *
 * return memory ID:uncachePoolMemPartId                      *
 * memPartAlloc()/memPartFree():allocate/free memory via      *
 * memory ID:uncachePoolMemPartId and size                    *
 * if BSP can gurantee the Memory allocated by cacheDmaMalloc *
 * cacheDmaFree() is noncached memory,we can call them directly*
 *************************************************************/

void *bcmos_dma_alloc(uint8_t device, uint32_t size)
{
    void * pBuffer;
#ifdef NON_CACHED_MEMORY_CREATED_BY_CACHEDMAMALLOC
    pBuffer = cacheDmaMalloc(size);
    return (pBuffer);
#else
    pBuffer = (void *)memPartAlloc(uncachePoolMemPartId, size));
    return (pBuffer);
#endif
}

/**************************************************************
 * Release DMA-able memory                                    *
 * related header file: memPartLib.h                          *
 * param[in]   device  Device id                              *
 * param[in]   ptr     Block pointer                          *
 * param[out]  none                                           *
 * return  none                                               *
 * comment: see above                                         *
 *************************************************************/
void bcmos_dma_free(uint8_t device, void *ptr)
{
#ifdef NON_CACHED_MEMORY_CREATED_BY_CACHEDMAMALLOC
    cacheDmaFree((void *)(ptr));
#else
    memPartFree(uncachePoolMemPartId,ptr);
#endif
    return;
}

/**************************************************************
 * Convert virtual address to physical address                *
 * related header file:for MIPS CPU	archMips.h                *
 * param[in]   va    Virtual address                          *
 * return - physical address va is mapped to                  *
**************************************************************/
unsigned long bcmos_virt_to_phys(void *va)
{
#ifdef HOST_CPU_IS_MIPS
  return (unsigned long)(K0_TO_PHYS(va));

#else
	/* this is an just an example,
	if you are not working with mips make sure you define the correct mapping for your system */
    return (unsigned long)(va);
#endif
}

/**************************************************************
 * Invalidate address area in data cache.                     *
 * related header file:  cacheLib.h                           *
 * param[in]   start   Address area start                     *
 * param[in]   size    Address area size                      *
 * return  none                                               *
 **************************************************************/
void bcmos_dcache_inv(void *start, uint32_t size)
{
    cacheInvalidate(DATA_CACHE,start, size);
    return;
}

/**************************************************************
 * Flush address area in data cache.                          *
 * related header file:  cacheLib.h                           *
 * param[in]   start   Address area start                     *
 * param[in]   size    Address area size                      *
 * return  none                                               *
**************************************************************/
void bcmos_dcache_flush(void *start, uint32_t size)
{
    cacheFlush(DATA_CACHE,start, size);
    return;
}

/**************************************************************
 * write value to PCI memory                                  *
 * related header file: bcmos_common2.h                       *
 * param[in]   address  pointer to Address                    *
 * param[in]   value    value                                 *
 * return  none                                               *
 * Comment re-use BCMOS_ENDIAN_CPU_TO_LITTLE_U32 which is from*
 * bcmos_common2.h,by this macro,can support both big endian  *
 * host and little endian host.move PCI access functions after*
 * bcmos_common2.h                                            *
 **************************************************************/
void bcm_pci_write32(volatile uint32_t *address, uint32_t value)
{
#ifdef HOST_PCIE_SWAP_NEEDED
    value = BCMOS_ENDIAN_CPU_TO_LITTLE_U32(value);
#endif
    *address = value;
}

/**************************************************************
 * read value to PCI memory                                   *
 * related header file: bcmos_common2.h                       *
 * param[in]   address  pointer to Address                    *
 * return  value                                              *
 * Comment see above                                          *
**************************************************************/
uint32_t bcm_pci_read32(const volatile uint32_t *address)
{
    uint32_t value;
    value = *address;
#ifdef HOST_PCIE_SWAP_NEEDED
    value = BCMOS_ENDIAN_LITTLE_TO_CPU_U32(value);
#endif
    return value;

}