thead-kernel/drivers/gpu/drm/verisilicon/vs_dc_mmu.c

// SPDX-License-Identifier: GPL-2.0
/*
 * Copyright (C) 2020 VeriSilicon Holdings Co., Ltd.
 */

#include <linux/device.h>
#include <linux/errno.h>
#include <linux/kernel.h>
#include <linux/mm.h>
#include <linux/mman.h>
#include <linux/pagemap.h>
#include <linux/slab.h>
#include <linux/dma-mapping.h>
#include <linux/vmalloc.h>

#include <asm/io.h>
#include <asm/delay.h>

#include "vs_dc_mmu.h"

static bool mmu_construct = false;

int _allocate_memory(u32 bytes, void **memory)
{
    void *mem = NULL;

    if (bytes == 0 || memory == NULL) {
        pr_err("%s has invalid arguments.\n", __FUNCTION__);
        return -EINVAL;
    }

    if (bytes > PAGE_SIZE) {
        mem = vmalloc(bytes);
    }
    else {
        mem = kmalloc(bytes, GFP_KERNEL);
    }

    if (!mem) {
        pr_err("%s out of memory.\n", __FUNCTION__);
        return -ENOMEM;
    }

    memset((u8 *)mem, 0, bytes);
    *memory = mem;

    return 0;
}

static int _create_mutex(void **mutex)
{
    int ret =0;

    if (mutex == NULL) {
        return -EINVAL;
    }

    ret = _allocate_memory(sizeof(struct mutex), mutex);
    if (ret)
        return ret;

    mutex_init(*(struct mutex **)mutex);

    return 0;
}

static int _acquire_mutex(void *mutex, u32 timeout)
{
    if (mutex == NULL) {
        pr_err("%s has invalid argument.\n", __FUNCTION__);
        return -EINVAL;
    }

    if (timeout == DC_INFINITE) {
        mutex_lock(mutex);
        return 0;
    }

    for (;;) {
        /* Try to acquire the mutex. */
        if (mutex_trylock(mutex)) {
            /* Success. */
            return 0;
        }

        if (timeout-- == 0) {
            break;
        }

        /* Wait for 1 millisecond. */
        udelay(1000);
    }

    return -ETIMEDOUT;
}

static int _release_mutex(void *mutex)
{
    if (mutex == NULL) {
        pr_err("%s has invalid argument.\n", __FUNCTION__);
        return -EINVAL;
    }

    mutex_unlock(mutex);

    return 0;
}

static u32 _mtlb_offset(u32 address)
{
    return (address & MMU_MTLB_MASK) >> MMU_MTLB_SHIFT;
}

static u32 _stlb_offset(u32 address)
{
    return (address & MMU_STLB_4K_MASK) >> MMU_STLB_4K_SHIFT;
}

static u32 _address_to_index(dc_mmu_pt mmu, u32 address)
{
    return _mtlb_offset(address) * MMU_STLB_4K_ENTRY_NUM + _stlb_offset(address);
}

static u32 _set_page(u32 page_address, u32 page_address_ext, bool writable)
{
    u32 entry = page_address
        /* AddressExt */
        | (page_address_ext << 4)
        /* Ignore exception */
        | (0 << 1)
        /* Present */
        | (1 << 0);

    if (writable) {
        /* writable */
        entry |= (1 << 2);
    }

    return entry;
}

static void _write_page_entry(u32 *page_entry, u32 entry_value)
{
    *page_entry = entry_value;
}

static u32 _read_page_entry(u32 *page_entry)
{
    return *page_entry;
}

int _allocate_stlb(dc_mmu_stlb_pt *stlb)
{
    dc_mmu_stlb_pt stlb_t = NULL;
    void *mem = NULL;

    mem = kzalloc(sizeof(dc_mmu_stlb), GFP_KERNEL);
    if (!mem)
        return -ENOMEM;

    stlb_t = (dc_mmu_stlb_pt)mem;

    stlb_t->size = MMU_STLB_4K_SIZE;

    *stlb = stlb_t;

    return 0;
}

int _allocate_all_stlb(struct device *dev, dc_mmu_stlb_pt *stlb)
{
    dc_mmu_stlb_pt stlb_t = NULL;
    void *mem = NULL;
    void *cookie = NULL;
    dma_addr_t dma_addr;
    size_t size;

    mem = kzalloc(sizeof(dc_mmu_stlb), GFP_KERNEL);
    if (!mem)
        return -ENOMEM;

    stlb_t = (dc_mmu_stlb_pt)mem;

    stlb_t->size = MMU_STLB_4K_SIZE * MMU_MTLB_ENTRY_NUM;
    size = PAGE_ALIGN(stlb_t->size);

    cookie = dma_alloc_wc(dev, size, &dma_addr, GFP_KERNEL);
    if (!cookie) {
        dev_err(dev, "Failed to alloc stlb buffer.\n");
        return -ENOMEM;
    }

    stlb_t->logical = cookie;
    stlb_t->physBase = (u64)dma_addr;
    memset(stlb_t->logical, 0, size);

    *stlb = stlb_t;

    return 0;
}

int _setup_process_address_space(struct device *dev, dc_mmu_pt mmu)
{
    u32 *map = NULL;
    u32 free, i;
    u32 dynamic_mapping_entries, address;
    dc_mmu_stlb_pt all_stlb;
    int ret =0;

    dynamic_mapping_entries = MMU_MTLB_ENTRY_NUM;
    mmu->dynamic_mapping_start = 0;
    mmu->page_table_size = dynamic_mapping_entries * MMU_STLB_4K_SIZE;

    mmu->page_table_entries = mmu->page_table_size / sizeof(u32);

    ret = _allocate_memory(mmu->page_table_size,
            (void **)&mmu->map_logical);
    if (ret) {
        pr_err("Failed to alloc mmu map buffer.\n");
        return ret;;
    }

    map = mmu->map_logical;

    /* Initialize free area*/
    free = mmu->page_table_entries;
    _write_page_entry(map, (free << 8) | DC_MMU_FREE);
    _write_page_entry(map + 1, ~0U);

    mmu->heap_list  = 0;
    mmu->free_nodes = false;

    ret = _allocate_all_stlb(dev, &all_stlb);
    if (ret)
        return ret;

    for (i = 0; i < dynamic_mapping_entries; i++) {
        dc_mmu_stlb_pt stlb;
        dc_mmu_stlb_pt *stlbs = (dc_mmu_stlb_pt *)mmu->stlbs;

        ret = _allocate_stlb(&stlb);
        if (ret) 
            return ret;

        stlb->physBase = all_stlb->physBase + i * MMU_STLB_4K_SIZE;
        stlb->logical = all_stlb->logical + i * MMU_STLB_4K_SIZE / sizeof(u32);

        stlbs[i] = stlb;
    }

    address = (u32)all_stlb->physBase;

    ret = _acquire_mutex(mmu->page_table_mutex, DC_INFINITE);
    if (ret)
        return ret;

    for (i = mmu->dynamic_mapping_start;
         i < mmu->dynamic_mapping_start + dynamic_mapping_entries;
         i++) {
        u32 mtlb_entry;

        mtlb_entry = address
               | MMU_MTLB_4K_PAGE
               | MMU_MTLB_PRESENT;

        address += MMU_STLB_4K_SIZE;

        /* Insert Slave TLB address to Master TLB entry.*/
        _write_page_entry(mmu->mtlb_logical + i, mtlb_entry);
    }

    _release_mutex(mmu->page_table_mutex);

    return 0;
}

/* MMU Construct */
int dc_mmu_construct(struct device *dev, dc_mmu_pt *mmu)
{
    dc_mmu_pt mmu_t = NULL;
    void *mem = NULL;
    void *cookie = NULL, *cookie_safe =NULL;
    dma_addr_t dma_addr, dma_addr_safe;
    u32 size = 0;
    int ret = 0;

    if (mmu_construct)
        return 0;

    mem = kzalloc(sizeof(dc_mmu), GFP_KERNEL);
    if (!mem)
        return -ENOMEM;

    mmu_t = (dc_mmu_pt)mem;
    mmu_t->mtlb_bytes = MMU_MTLB_SIZE;
    size = PAGE_ALIGN(mmu_t->mtlb_bytes);

    /* Allocate MTLB */
    cookie = dma_alloc_wc(dev, size, &dma_addr, GFP_KERNEL);
    if (!cookie) {
        dev_err(dev, "Failed to alloc mtlb buffer.\n");
        return -ENOMEM;
    }

    mmu_t->mtlb_logical = cookie;
    mmu_t->mtlb_physical = (u64)dma_addr;
    memset(mmu_t->mtlb_logical, 0, size);

    size = MMU_MTLB_ENTRY_NUM * sizeof(dc_mmu_stlb_pt);

    ret = _allocate_memory(size, &mmu_t->stlbs);
    if (ret)
        return ret;

    ret = _create_mutex(&mmu_t->page_table_mutex);
    if (ret)
        return ret;

    mmu_t->mode = MMU_MODE_1K;

    ret = _setup_process_address_space(dev, mmu_t);
    if (ret)
        return ret;

    /* Allocate safe page */
    cookie_safe = dma_alloc_wc(dev, 4096, &dma_addr_safe, GFP_KERNEL);
    if (!cookie_safe) {
        dev_err(dev, "Failed to alloc safe page.\n");
        return -ENOMEM;
    }

    mmu_t->safe_page_logical = cookie_safe;
    mmu_t->safe_page_physical = (u64)dma_addr_safe;
    memset(mmu_t->safe_page_logical, 0, size);

    *mmu = mmu_t;
    mmu_construct = true;

    return 0;
}

int dc_mmu_get_page_entry(dc_mmu_pt mmu, u32 address, u32 **page_table)
{
    dc_mmu_stlb_pt stlb;
    dc_mmu_stlb_pt *stlbs = (dc_mmu_stlb_pt *)mmu->stlbs;
    u32 mtlb_offset = _mtlb_offset(address);
    u32 stlb_offset = _stlb_offset(address);

    stlb = stlbs[mtlb_offset - mmu->dynamic_mapping_start];
    if (stlb == NULL) {
        pr_err("BUG: invalid stlb,  mmu=%p stlbs=%p  mtlb_offset=0x%x %s(%d)\n",
            mmu, stlbs ,mtlb_offset,__FUNCTION__,__LINE__);
        return -ENXIO;
    }

    *page_table = &stlb->logical[stlb_offset];

    return 0;
}

int _link(dc_mmu_pt mmu, u32 index, u32 node)
{
    if (index >= mmu->page_table_entries) {
        mmu->heap_list = node;
    }
    else {
        u32 *map = mmu->map_logical;

        switch (DC_ENTRY_TYPE(_read_page_entry(&map[index]))) {
        case DC_MMU_SINGLE:
            /* Previous is a single node, link to it*/
            _write_page_entry(&map[index], (node << 8) | DC_MMU_SINGLE);
            break;
        case DC_MMU_FREE:
            /* Link to FREE TYPE node */
            _write_page_entry(&map[index + 1], node);
            break;
        default:
            pr_err("MMU table corrupted at index %u!", index);
            return -EINVAL;
        }
    }

    return 0;
}

int _add_free(dc_mmu_pt mmu, u32 index, u32 node, u32 count)
{
    u32 *map = mmu->map_logical;

    if (count == 1) {
        /* Initialize a single page node */
        _write_page_entry(map + node, DC_SINGLE_PAGE_NODE_INITIALIZE | DC_MMU_SINGLE);
    }
    else {
        /* Initialize the FREE node*/
        _write_page_entry(map + node, (count << 8) | DC_MMU_FREE);
        _write_page_entry(map + node + 1, ~0U);
    }

    return _link(mmu, index, node);
}

/* Collect free nodes */
int _collect(dc_mmu_pt mmu)
{
    u32 *map = mmu->map_logical;
    u32 count = 0, start = 0, i = 0;
    u32 previous = ~0U;
    int ret = 0;

    mmu->heap_list = ~0U;
    mmu->free_nodes = false;

    /* Walk the entire page table */
    for (i = 0; i < mmu->page_table_entries; i++) {
        switch (DC_ENTRY_TYPE(_read_page_entry(&map[i]))) {
        case DC_MMU_SINGLE:
            if (count++ == 0) {
                /* Set new start node */
                start = i;
            }
            break;
        case DC_MMU_FREE:
            if (count == 0) {
                /* Set new start node */
                start = i;
            }

            count += _read_page_entry(&map[i]) >> 8;
            /* Advance the index of the page table */
            i += (_read_page_entry(&map[i]) >> 8) - 1;
            break;
        case DC_MMU_USED:
            /* Meet used node, start to collect */
            if (count > 0) {
                /* Add free node to list*/
                ret = _add_free(mmu, previous, start, count);
                if (ret)
                    return ret;
                /* Reset previous unused node index */
                previous = start;
                count = 0;
            }
            break;
        default:
            pr_err("MMU page table corrupted at index %u!", i);
            return -EINVAL;
        }
    }

    /* If left node is an open node. */
    if (count > 0) {
        ret = _add_free(mmu, previous, start, count);
        if (ret)
            return ret;
    }

    return 0;
}

int _fill_page_table(u32 *page_table, u32 page_count, u32 entry_value)
{
    u32 i;

    for (i = 0; i < page_count; i++) {
        _write_page_entry(page_table + i, entry_value);
    }

    return 0;
}

int dc_mmu_allocate_pages(dc_mmu_pt mmu, u32 page_count, u32 *address)
{
    bool got = false, acquired = false;
    u32 *map;
    u32 index = 0, vaddr, left;
    u32 previous = ~0U;
    u32 mtlb_offset, stlb_offset;
    int ret = 0;

    if (page_count == 0 || page_count > mmu->page_table_entries) {
        pr_err("%s has invalid arguments.\n", __FUNCTION__);
        return -EINVAL;
    }

    _acquire_mutex(mmu->page_table_mutex, DC_INFINITE);
    acquired = true;

    for (map = mmu->map_logical; !got;) {
        for (index = mmu->heap_list; !got && (index < mmu->page_table_entries);) {
            switch (DC_ENTRY_TYPE(_read_page_entry(&map[index]))) {
            case DC_MMU_SINGLE:
                if (page_count == 1) {
                    got = true;
                }
                else {
                    /* Move to next node */
                    previous = index;
                    index = _read_page_entry(&map[index]) >> 8;
                }
                break;
            case DC_MMU_FREE:
                if (page_count <= (_read_page_entry(&map[index]) >> 8)) {
                    got = true;
                }
                else {
                    /* Move to next node */
                    previous = index;
                    index = _read_page_entry(&map[index + 1]);
                }
                break;
            default:
                /* Only link SINGLE and FREE node */
                pr_err("MMU table corrupted at index %u!", index);
                ret = -EINVAL;
                goto OnError;
            }
        }

        /* If out of index */
        if (index >= mmu->page_table_entries) {
            if (mmu->free_nodes) {
                /* Collect the free node */
                ret = _collect(mmu);
                if (ret)
                    goto OnError;
            }
            else {
                ret = -ENODATA;
                goto OnError;
            }
        }
    }

    switch (DC_ENTRY_TYPE(_read_page_entry(&map[index]))) {
    case DC_MMU_SINGLE:
        /* Unlink single node from node list */
        ret = _link(mmu, previous, _read_page_entry(&map[index]) >> 8);
        if (ret)
            goto OnError;
        break;

    case DC_MMU_FREE:
        left = (_read_page_entry(&map[index]) >> 8) - page_count;
        switch (left) {
        case 0:
            /* Unlink the entire FREE type node */
            ret = _link(mmu, previous, _read_page_entry(&map[index + 1]));
            if (ret)
                goto OnError;
            break;
        case 1:
            /* Keep the map[index] as a single node,
             * mark the left as used
             */
            _write_page_entry(&map[index], 
                (_read_page_entry(&map[index + 1]) << 8) |
                DC_MMU_SINGLE);
            index++;
            break;
        default:
            /* FREE type node left */
            _write_page_entry(&map[index],
                      (left << 8) | DC_MMU_FREE);
            index += left;
            break;
        }
        break;
    default:
        /* Only link SINGLE and FREE node */
        pr_err("MMU table corrupted at index %u!", index);
        ret = -EINVAL;
        goto OnError;
    }

    /* Mark node as used */
    ret = _fill_page_table(&map[index], page_count, DC_MMU_USED);
    if (ret)
        goto OnError;

    _release_mutex(mmu->page_table_mutex);

    mtlb_offset = index / MMU_STLB_4K_ENTRY_NUM + mmu->dynamic_mapping_start;
    stlb_offset = index % MMU_STLB_4K_ENTRY_NUM;

    vaddr = (mtlb_offset << MMU_MTLB_SHIFT) | (stlb_offset << MMU_STLB_4K_SHIFT);

    if (address != NULL) {
        *address = vaddr;
    }

    return 0;

OnError:
    if (acquired) {
        _release_mutex(mmu->page_table_mutex);
    }

    return ret;
}

int dc_mmu_free_pages(dc_mmu_pt mmu, u32 address, u32 page_count)
{
    u32 *node;

    if (page_count == 0)
        return -EINVAL;

    node = mmu->map_logical + _address_to_index(mmu, address);

    _acquire_mutex(mmu->page_table_mutex, DC_INFINITE);

    if (page_count == 1) {
        /* Mark the Single page node free */
        _write_page_entry(node, DC_SINGLE_PAGE_NODE_INITIALIZE | DC_MMU_SINGLE);
    }
    else {
         /* Mark the FREE type node free */
        _write_page_entry(node, (page_count << 8) | DC_MMU_FREE);
        _write_page_entry(node + 1, ~0U);
    }

    mmu->free_nodes = true;

    _release_mutex(mmu->page_table_mutex);

    return 0;
}

int dc_mmu_set_page(dc_mmu_pt mmu, u64 page_address, u32 *page_entry)
{
    u32 address_ext;
    u32 address;

    if (page_entry == NULL || (page_address & 0xFFF)) {
        return -EINVAL;
    }

    /* [31:0]. */
    address = (u32)(page_address & 0xFFFFFFFF);
    /* [39:32]. */
    address_ext = (u32)((page_address >> 32) & 0xFF);

    _write_page_entry(page_entry, _set_page(address, address_ext, true));

    return 0;
}

int dc_mmu_map_memory(dc_mmu_pt mmu, u64 physical, u32 page_count,
              u32 *address, bool continuous, bool security)
{
    u32 virutal_address, i= 0;
    u32 mtlb_num, mtlb_entry, mtlb_offset;
    bool allocated = false;
    int ret = 0;

    ret = dc_mmu_allocate_pages(mmu, page_count, &virutal_address);
    if (ret)
        goto OnError;

    *address = virutal_address;
    allocated = true;

    /*Fill mtlb security bit*/
    mtlb_num = _mtlb_offset(virutal_address + page_count * MMU_PAGE_4K_SIZE - 1) -
        _mtlb_offset(virutal_address) + 1;
    mtlb_offset = _mtlb_offset(virutal_address);
    mtlb_entry = mmu->mtlb_logical[mtlb_offset];

    for (i = 0; i < mtlb_num ; i++) {
        mtlb_entry = mmu->mtlb_logical[mtlb_offset + i];
        if(security) {
            mtlb_entry = mtlb_entry
                       | MMU_MTLB_SECURITY
                       | MMU_MTLB_EXCEPTION;
            _write_page_entry(&mmu->mtlb_logical[mtlb_offset + i], mtlb_entry);
        } else {
            mtlb_entry = mtlb_entry & (~MMU_MTLB_SECURITY);
            _write_page_entry(&mmu->mtlb_logical[mtlb_offset + i], mtlb_entry);
        }
    }

    /* Fill in page table */
    for (i = 0; i < page_count; i++) {
        u64 page_phy;
        u32 *page_entry;
        struct page **pages;

        if (continuous == true) {
            page_phy = physical + i * MMU_PAGE_4K_SIZE;
        }
        else {
            pages = (struct page **)physical;
            page_phy = page_to_phys(pages[i]);
        }

        ret = dc_mmu_get_page_entry(mmu, virutal_address, &page_entry);
        if (ret)
            goto OnError;

        /* Write the page address to the page entry */
        ret = dc_mmu_set_page(mmu, page_phy, page_entry);
        if (ret)
            goto OnError;

        /* Get next page */
        virutal_address += MMU_PAGE_4K_SIZE;
    }

    return 0;

OnError:
    if (allocated)
        dc_mmu_free_pages(mmu, virutal_address, page_count);
    pr_info("%s fail!\n", __FUNCTION__);

    return ret;
}

int dc_mmu_unmap_memory(dc_mmu_pt mmu, u32 gpu_address, u32 page_count)
{
    return dc_mmu_free_pages(mmu, gpu_address, page_count);
}