/*
 * Copyright 2017 Broadcom
 * 
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License, version 2, as
 * published by the Free Software Foundation (the "GPL").
 * 
 * This program is distributed in the hope that it will be useful, but
 * WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 * General Public License version 2 (GPLv2) for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * version 2 (GPLv2) along with this source code.
 */
/*
 * $Id: mpool.c,v 1.18 Broadcom SDK $
 * $Copyright: (c) 2005 Broadcom Corp.
 * All Rights Reserved.$
 */

#include <mpool.h>

#ifdef __KERNEL__

/* 
 * Abstractions used when compiling for Linux kernel mode. 
 */

#include <lkm.h>

/*
 * We cannot use the linux kernel SAL for MALLOC/FREE because 
 * the current implementation of sal_alloc() allocates memory 
 * out of an mpool created by this module...
 */
#define MALLOC(x) kmalloc(x, GFP_ATOMIC)
#define FREE(x) kfree(x)

static spinlock_t _mpool_lock;
#define MPOOL_LOCK_INIT() spin_lock_init(&_mpool_lock)
#define MPOOL_LOCK() unsigned long flags; spin_lock_irqsave(&_mpool_lock, flags)
#define MPOOL_UNLOCK() spin_unlock_irqrestore(&_mpool_lock, flags)

#else /* !__KERNEL__*/

/* 
 * Abstractions used when compiling for Linux user mode. 
 */

#include <stdlib.h>
#include <sal/core/sync.h>

#define MALLOC(x) malloc(x)
#define FREE(x) free(x)

static sal_sem_t _mpool_lock;
#define MPOOL_LOCK_INIT() _mpool_lock = sal_sem_create("mpool_lock", 1, 1)
#define MPOOL_LOCK() sal_sem_take(_mpool_lock, sal_sem_FOREVER)
#define MPOOL_UNLOCK() sal_sem_give(_mpool_lock)

#endif /* __KERNEL__ */

/* Allow external override for system cache line size */
#ifndef BCM_CACHE_LINE_BYTES
#ifdef L1_CACHE_BYTES
#define BCM_CACHE_LINE_BYTES L1_CACHE_BYTES
#else
#define BCM_CACHE_LINE_BYTES 128 /* Should be fine on most platforms */
#endif
#endif

#define MPOOL_BUF_SIZE               1024
#define MPOOL_BUF_ALLOC_COUNT_MAX      16

typedef struct mpool_mem_s {
    unsigned char *address;
    int size;
    struct mpool_mem_s *prev;
    struct mpool_mem_s *next;
} mpool_mem_t;

static int _buf_alloc_count;
static mpool_mem_t *mpool_buf[MPOOL_BUF_ALLOC_COUNT_MAX];
static mpool_mem_t *free_list;

#define ALLOC_INIT_MPOOL_BUF(ptr) \
        ptr = MALLOC((sizeof(mpool_mem_t) * MPOOL_BUF_SIZE)); \
        if (ptr) { \
            int i; \
            for (i = 0; i < MPOOL_BUF_SIZE - 1; i++) { \
                ptr[i].next = &ptr[i+1]; \
            } \
            ptr[MPOOL_BUF_SIZE - 1].next = NULL; \
            free_list = &ptr[0]; \
        }

/*
 * Function: mpool_init
 *
 * Purpose:
 *    Initialize mpool lock.
 * Parameters:
 *    None
 * Returns:
 *    Always 0
 */
int 
mpool_init(void)
{
    MPOOL_LOCK_INIT();
    return 0;
}

#ifdef TRACK_DMA_USAGE
static int _dma_mem_used = 0;
#endif

/*
 * Function: mpool_alloc
 *
 * Purpose:
 *    Allocate memory block from mpool.
 * Parameters:
 *    pool - mpool handle (from mpool_create)
 *    size - size of memory block to allocate
 * Returns:
 *    Pointer to allocated memory block or NULL if allocation fails.
 */
void *
mpool_alloc(mpool_handle_t pool, int size)
{
    mpool_mem_t *ptr = pool, *newptr = NULL;
    int mod;

    MPOOL_LOCK();

    if (size < BCM_CACHE_LINE_BYTES) {
        size = BCM_CACHE_LINE_BYTES;
    }

    mod = size & (BCM_CACHE_LINE_BYTES - 1);
    if (mod != 0 ) {
        size += (BCM_CACHE_LINE_BYTES - mod);
    }
    while (ptr && ptr->next) {
        if (ptr->next->address - (ptr->address + ptr->size) >= size) {
            break;
        }
        ptr = ptr->next;
    }
  
    if (!(ptr && ptr->next)) {
        MPOOL_UNLOCK();
        return NULL;
    }

    if (!free_list) {
        if (_buf_alloc_count == MPOOL_BUF_ALLOC_COUNT_MAX) {
            MPOOL_UNLOCK();
            return NULL;
        }

        ALLOC_INIT_MPOOL_BUF(mpool_buf[_buf_alloc_count]);

        if (mpool_buf[_buf_alloc_count] == NULL) {
            MPOOL_UNLOCK();
            return NULL;
        }

        _buf_alloc_count++;
    }

    newptr = free_list;
    free_list = free_list->next;
  
    newptr->address = ptr->address + ptr->size;
    newptr->size = size;
    newptr->next = ptr->next;
    newptr->prev = ptr;
    ptr->next->prev = newptr;
    ptr->next = newptr;
#ifdef TRACK_DMA_USAGE
    _dma_mem_used += size;
#endif

    MPOOL_UNLOCK();
    return newptr->address;
}


/*
 * Function: mpool_free
 *
 * Purpose:
 *    Free memory block allocated from mpool..
 * Parameters:
 *    pool - mpool handle (from mpool_create)
 *    addr - address of memory block to free
 * Returns:
 *    Nothing
 */
void 
mpool_free(mpool_handle_t pool, void *addr)
{
    unsigned char *address = (unsigned char *)addr;  
    mpool_mem_t *head = pool, *ptr = NULL;

    MPOOL_LOCK();

    if (!(head && head->prev)) {
        MPOOL_UNLOCK();
        return;
    }

    ptr = head->prev->prev;

    while (ptr && (ptr != head)) {
        if (ptr->address == address) {
#ifdef TRACK_DMA_USAGE
            _dma_mem_used -= ptr->size;
#endif
            ptr->prev->next = ptr->next;
            ptr->next->prev = ptr->prev;
            ptr->next = free_list;
            free_list = ptr;
            break;
        }
        ptr = ptr->prev;
    }

    MPOOL_UNLOCK();
}

/*
 * Function: mpool_create
 *
 * Purpose:
 *    Create and initialize mpool control structures.
 * Parameters:
 *    base_ptr - pointer to mpool memory block
 *    size - total size of mpool memory block
 * Returns:
 *    mpool handle
 * Notes
 *    The mpool handle returned must be used for subsequent
 *    memory allocations from the mpool.
 */
mpool_handle_t
mpool_create(void *base_ptr, int size)
{
    mpool_mem_t *head, *tail;
    int mod = (int)(((unsigned long)base_ptr) & (BCM_CACHE_LINE_BYTES - 1));
    int i;

    MPOOL_LOCK();

    for (i = 0; i < MPOOL_BUF_ALLOC_COUNT_MAX; i++) {
        mpool_buf[i] = NULL;
    }

    _buf_alloc_count = 0;

    ALLOC_INIT_MPOOL_BUF(mpool_buf[_buf_alloc_count]);

    if (mpool_buf[_buf_alloc_count] == NULL) {
        MPOOL_UNLOCK();
        return NULL;
    }

    _buf_alloc_count++;

    if (mod) {
        base_ptr = (char*)base_ptr + (BCM_CACHE_LINE_BYTES - mod);
        size -= (BCM_CACHE_LINE_BYTES - mod);
    }
    size &= ~(BCM_CACHE_LINE_BYTES - 1);

    head = free_list;
    free_list = free_list->next;
    tail = free_list;
    free_list = free_list->next;

    head->size = tail->size = 0;
    head->address = base_ptr;
    tail->address = head->address + size;
    head->prev = tail;
    head->next = tail;
    tail->prev = head;
    tail->next = NULL;

    MPOOL_UNLOCK();
    return head;
}

/*
 * Function: mpool_destroy
 *
 * Purpose:
 *    Free mpool control structures.
 * Parameters:
 *    pool - mpool handle (from mpool_create)
 * Returns:
 *    Always 0
 */
int
mpool_destroy(mpool_handle_t pool)
{
    int i;

    MPOOL_LOCK();

    if ((mpool_mem_t *)pool != mpool_buf[0]) {
        MPOOL_UNLOCK();
        return 0;
    }

    for (i = 0; i < MPOOL_BUF_ALLOC_COUNT_MAX; i++) {
        if (mpool_buf[i]) {
            FREE(mpool_buf[i]);
            mpool_buf[i] = NULL;
        }
    }

    MPOOL_UNLOCK();

    return 0;
}

/*
 * Function: mpool_usage
 *
 * Purpose:
 *    Report total sum of allocated mpool memory.
 * Parameters:
 *    pool - mpool handle (from mpool_create)
 * Returns:
 *    Number of bytes currently allocated using mpool_alloc.
 */
int
mpool_usage(mpool_handle_t pool)
{
    int usage = 0;
    mpool_mem_t *ptr;

    MPOOL_LOCK();

    for (ptr = pool; ptr; ptr = ptr->next) {
        usage += ptr->size;
    }

    MPOOL_UNLOCK();

    return usage;
}