1 /* 2 * Copyright 2004-2009, Marcus Overhagen. All rights reserved. 3 * Distributed under the terms of the MIT License. 4 */ 5 #include "util.h" 6 7 #include <KernelExport.h> 8 #include <OS.h> 9 #include <vm/vm.h> 10 #include <string.h> 11 12 13 #define TRACE(a...) dprintf("ahci: " a) 14 #define ERROR(a...) dprintf("ahci: " a) 15 16 17 static inline uint32 18 round_to_pagesize(uint32 size) 19 { 20 return (size + B_PAGE_SIZE - 1) & ~(B_PAGE_SIZE - 1); 21 } 22 23 24 area_id 25 alloc_mem(void **virt, phys_addr_t *phy, size_t size, uint32 protection, 26 const char *name) 27 { 28 physical_entry pe; 29 void * virtadr; 30 area_id areaid; 31 status_t rv; 32 33 TRACE("allocating %ld bytes for %s\n", size, name); 34 35 size = round_to_pagesize(size); 36 areaid = create_area(name, &virtadr, B_ANY_KERNEL_ADDRESS, size, 37 B_CONTIGUOUS, protection); 38 if (areaid < B_OK) { 39 ERROR("couldn't allocate area %s\n", name); 40 return B_ERROR; 41 } 42 rv = get_memory_map(virtadr, size, &pe, 1); 43 if (rv < B_OK) { 44 delete_area(areaid); 45 ERROR("couldn't get mapping for %s\n", name); 46 return B_ERROR; 47 } 48 if (virt) 49 *virt = virtadr; 50 if (phy) 51 *phy = pe.address; 52 TRACE("area = %ld, size = %ld, virt = %p, phy = %#" B_PRIxPHYSADDR "\n", 53 areaid, size, virtadr, pe.address); 54 return areaid; 55 } 56 57 58 area_id 59 map_mem(void **virt, phys_addr_t phy, size_t size, uint32 protection, 60 const char *name) 61 { 62 uint32 offset; 63 phys_addr_t phyadr; 64 void *mapadr; 65 area_id area; 66 67 TRACE("mapping physical address %#" B_PRIxPHYSADDR " with %" B_PRIuSIZE 68 " bytes for %s\n", phy, size, name); 69 70 offset = phy & (B_PAGE_SIZE - 1); 71 phyadr = phy - offset; 72 size = round_to_pagesize(size + offset); 73 area = map_physical_memory(name, phyadr, size, 74 B_ANY_KERNEL_BLOCK_ADDRESS, protection, &mapadr); 75 if (area < B_OK) { 76 ERROR("mapping '%s' failed, error 0x%lx (%s)\n", name, area, strerror(area)); 77 return area; 78 } 79 80 *virt = (char *)mapadr + offset; 81 82 TRACE("physical = %#" B_PRIxPHYSADDR ", virtual = %p, offset = %ld, " 83 "phyadr = %#" B_PRIxPHYSADDR ", mapadr = %p, size = %ld, area = " 84 "0x%08lx\n", phy, *virt, offset, phyadr, mapadr, size, area); 85 86 return area; 87 } 88 89 90 status_t 91 sg_memcpy(const physical_entry *sgTable, int sgCount, const void *data, 92 size_t dataSize) 93 { 94 int i; 95 for (i = 0; i < sgCount && dataSize > 0; i++) { 96 size_t size = min_c(dataSize, sgTable[i].size); 97 98 TRACE("sg_memcpy phyAddr %#" B_PRIxPHYSADDR ", size %lu\n", 99 sgTable[i].address, size); 100 101 vm_memcpy_to_physical(sgTable[i].address, data, size, false); 102 103 data = (char *)data + size; 104 dataSize -= size; 105 } 106 if (dataSize != 0) 107 return B_ERROR; 108 return B_OK; 109 } 110 111 112 void 113 swap_words(void *data, size_t size) 114 { 115 uint16 *word = (uint16*)data; 116 size_t count = size / 2; 117 while (count--) { 118 *word = (*word << 8) | (*word >> 8); 119 word++; 120 } 121 } 122