/*
 * Copyright 2004-2009, Marcus Overhagen. All rights reserved.
 * Distributed under the terms of the MIT License.
 */
#include "util.h"

#include <KernelExport.h>
#include <OS.h>
#include <vm/vm.h>
#include <string.h>


#define TRACE(a...) dprintf("ahci: " a)
#define ERROR(a...) dprintf("ahci: " a)


static inline uint32
round_to_pagesize(uint32 size)
{
        return (size + B_PAGE_SIZE - 1) & ~(B_PAGE_SIZE - 1);
}


area_id
alloc_mem(void **virt, phys_addr_t *phy, size_t size, uint32 protection,
        const char *name)
{
        physical_entry pe;
        void * virtadr;
        area_id areaid;
        status_t rv;

        TRACE("allocating %ld bytes for %s\n", size, name);

        size = round_to_pagesize(size);
        areaid = create_area(name, &virtadr, B_ANY_KERNEL_ADDRESS, size,
                B_CONTIGUOUS, protection);
        if (areaid < B_OK) {
                ERROR("couldn't allocate area %s\n", name);
                return B_ERROR;
        }
        rv = get_memory_map(virtadr, size, &pe, 1);
        if (rv < B_OK) {
                delete_area(areaid);
                ERROR("couldn't get mapping for %s\n", name);
                return B_ERROR;
        }
        if (virt)
                *virt = virtadr;
        if (phy)
                *phy = pe.address;
        TRACE("area = %" B_PRId32 ", size = %ld, virt = %p, phy = %#" B_PRIxPHYSADDR "\n",
                areaid, size, virtadr, pe.address);
        return areaid;
}


area_id
map_mem(void **virt, phys_addr_t phy, size_t size, uint32 protection,
        const char *name)
{
        uint32 offset;
        phys_addr_t phyadr;
        void *mapadr;
        area_id area;

        TRACE("mapping physical address %#" B_PRIxPHYSADDR " with %" B_PRIuSIZE
                " bytes for %s\n", phy, size, name);

        offset = phy & (B_PAGE_SIZE - 1);
        phyadr = phy - offset;
        size = round_to_pagesize(size + offset);
        area = map_physical_memory(name, phyadr, size,
                B_ANY_KERNEL_BLOCK_ADDRESS, protection, &mapadr);
        if (area < B_OK) {
                ERROR("mapping '%s' failed, error 0x%" B_PRIx32 " (%s)\n", name,
                        area, strerror(area));
                return area;
        }

        *virt = (char *)mapadr + offset;

        TRACE("physical = %#" B_PRIxPHYSADDR ", virtual = %p, offset = %"
                B_PRId32 ", phyadr = %#" B_PRIxPHYSADDR ", mapadr = %p, size = %"
                B_PRIuSIZE ", area = 0x%08" B_PRIx32 "\n", phy, *virt, offset, phyadr,
                mapadr, size, area);

        return area;
}


status_t
sg_memcpy(const physical_entry *sgTable, int sgCount, const void *data,
        size_t dataSize)
{
        if (sgTable == NULL || data == NULL) {
                if (dataSize == 0)
                        return B_OK;
                return B_ERROR;
        }
        int i;
        for (i = 0; i < sgCount && dataSize > 0; i++) {
                size_t size = min_c(dataSize, sgTable[i].size);

                TRACE("sg_memcpy phyAddr %#" B_PRIxPHYSADDR ", size %lu\n",
                        sgTable[i].address, size);

                vm_memcpy_to_physical(sgTable[i].address, data, size, false);

                data = (char *)data + size;
                dataSize -= size;
        }
        if (dataSize != 0)
                return B_ERROR;
        return B_OK;
}


void
swap_words(void *data, size_t size)
{
        uint16 *word = (uint16*)data;
        size_t count = size / 2;
        while (count--) {
                *word = (*word << 8) | (*word >> 8);
                word++;
        }
}