* 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++;
}
}
