berlin_tools/bootloader/common/drivers/usb_ctrl/dwc2/diag_buffpool.c

/*
 * Copyright (C) 2018 Synaptics Incorporated. All rights reserved.
 *
 * 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.
 *
 * INFORMATION CONTAINED IN THIS DOCUMENT IS PROVIDED "AS-IS," AND
 * SYNAPTICS EXPRESSLY DISCLAIMS ALL EXPRESS AND IMPLIED WARRANTIES,
 * INCLUDING ANY IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE, AND ANY WARRANTIES OF NON-INFRINGEMENT OF ANY
 * INTELLECTUAL PROPERTY RIGHTS. IN NO EVENT SHALL SYNAPTICS BE LIABLE
 * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, PUNITIVE, OR
 * CONSEQUENTIAL DAMAGES ARISING OUT OF OR IN CONNECTION WITH THE USE
 * OF THE INFORMATION CONTAINED IN THIS DOCUMENT, HOWEVER CAUSED AND
 * BASED ON ANY THEORY OF LIABILITY, WHETHER IN AN ACTION OF CONTRACT,
 * NEGLIGENCE OR OTHER TORTIOUS ACTION, AND EVEN IF SYNAPTICS WAS
 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. IF A TRIBUNAL OF
 * COMPETENT JURISDICTION DOES NOT PERMIT THE DISCLAIMER OF DIRECT
 * DAMAGES OR ANY OTHER DAMAGES, SYNAPTICS' TOTAL CUMULATIVE LIABILITY
 * TO ANY PARTY SHALL NOT EXCEED ONE HUNDRED U.S. DOLLARS.
 */
#include "diag_common.h"

extern void *malloc(size_t size);

#define LOCK(irq_save, fiq_save, lock)              { irq_save = DisableIRQ(); fiq_save = DisableFIQ(); spin_lock(lock);}
#define UNLOCK(irq_save, fiq_save, lock)            { spin_unlock(lock); RestoreFIQ(fiq_save); RestoreIRQ(irq_save);}

typedef struct
{
    unsigned int    start;
    unsigned int    size;
    unsigned int    block_size;
    unsigned int    block_count;
    unsigned char*  map;
    unsigned int    mutex;
} mem_buffer_manager_t;
static mem_buffer_manager_t mem_buffer_manager;

int diag_buffpool_init(unsigned int start, int size, int block_size)
{
    mem_buffer_manager.start        = start;
    mem_buffer_manager.size         = size;
    mem_buffer_manager.block_size   = block_size;
    mem_buffer_manager.block_count  = size/block_size;
    mem_buffer_manager.map          = (unsigned char*)malloc(ALIGN(mem_buffer_manager.block_count, 4));
    DIAG_ASSERT(mem_buffer_manager.map != 0);
    memset(mem_buffer_manager.map, 0, mem_buffer_manager.block_count);
    spin_lock_init((int*)(&mem_buffer_manager.mutex));
    dbg_printf(PRN_INFO, "diag_buffer_pool start:0x%08x, size:0x%08x, block size:0x%x, map addr:0x%08x\n",
                start, size, block_size, mem_buffer_manager.map);
    return 0;
}

unsigned int diag_buffpool_alloc(int size, unsigned char tag)
{
    unsigned int irq_save;
    unsigned int fiq_save;
    unsigned char* p = mem_buffer_manager.map;
    unsigned char* p_end = p + mem_buffer_manager.block_count;
    int blocks_req = (size + mem_buffer_manager.block_size - 1)/mem_buffer_manager.block_size;
    int continue_free_count = 0;
    int addr;
    LOCK(irq_save, fiq_save, (int*)(&mem_buffer_manager.mutex));
    while(p < p_end)
    {
        if (*p == 0)
        {
            continue_free_count++;
            if (continue_free_count >= blocks_req)
            {
                break;
            }
        }
        else
        {
            continue_free_count = 0;
        }
        p++;
    }
    if (continue_free_count >= blocks_req)
    {
        while(continue_free_count-- > 1)
        {
            *p-- = tag;
        }
        *p = tag | 0x80; // start flag
        addr = mem_buffer_manager.start + (p-mem_buffer_manager.map)*mem_buffer_manager.block_size;
    }
    else
    {
        addr = 0; // no buffer
    }
    UNLOCK(irq_save, fiq_save, (int*)(&mem_buffer_manager.mutex));
    if (addr == 0)
    {
        dbg_printf(PRN_RES, "diag_buffer_pool out of memory\n");
    }
    return addr;
}

unsigned int diag_buffpool_free(unsigned int addr, unsigned char tag)
{
    unsigned int irq_save;
    unsigned int fiq_save;
    unsigned char* p;
    unsigned char* p_end = mem_buffer_manager.map + mem_buffer_manager.block_count;
    int count = 0;
    if (0 != (addr & (mem_buffer_manager.block_size-1)))
    {
        dbg_printf(PRN_RES, " address wrong, addr:0x%08x\n", addr);
        return -1;
    }
    // check
    p = mem_buffer_manager.map + (addr - mem_buffer_manager.start)/mem_buffer_manager.block_size;
    if (*p != (tag|0x80))
    {
        dbg_printf(PRN_RES, " tag wrong, tag:0x%02x, expect:0x%02x\n", *p, (tag|0x80));
        return -1;
    }
    *p++ = 0;
    count++;
    LOCK(irq_save, fiq_save, (int*)(&mem_buffer_manager.mutex));
    while(*p == tag && p < p_end)
    {
        *p = 0;
        p++;
        count++;
    }
    UNLOCK(irq_save, fiq_save, (int*)(&mem_buffer_manager.mutex));
    return 0;
}