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 = %" B_PRId32 ", 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%" B_PRIx32 " (%s)\n", name, 77 area, strerror(area)); 78 return area; 79 } 80 81 *virt = (char *)mapadr + offset; 82 83 TRACE("physical = %#" B_PRIxPHYSADDR ", virtual = %p, offset = %" 84 B_PRId32 ", phyadr = %#" B_PRIxPHYSADDR ", mapadr = %p, size = %" 85 B_PRIuSIZE ", area = 0x%08" B_PRIx32 "\n", phy, *virt, offset, phyadr, 86 mapadr, size, area); 87 88 return area; 89 } 90 91 92 status_t 93 sg_memcpy(const physical_entry *sgTable, int sgCount, const void *data, 94 size_t dataSize) 95 { 96 if (sgTable == NULL || data == NULL) { 97 if (dataSize == 0) 98 return B_OK; 99 return B_ERROR; 100 } 101 int i; 102 for (i = 0; i < sgCount && dataSize > 0; i++) { 103 size_t size = min_c(dataSize, sgTable[i].size); 104 105 TRACE("sg_memcpy phyAddr %#" B_PRIxPHYSADDR ", size %lu\n", 106 sgTable[i].address, size); 107 108 vm_memcpy_to_physical(sgTable[i].address, data, size, false); 109 110 data = (char *)data + size; 111 dataSize -= size; 112 } 113 if (dataSize != 0) 114 return B_ERROR; 115 return B_OK; 116 } 117 118 119 void 120 swap_words(void *data, size_t size) 121 { 122 uint16 *word = (uint16*)data; 123 size_t count = size / 2; 124 while (count--) { 125 *word = (*word << 8) | (*word >> 8); 126 word++; 127 } 128 } 129 130 131 int 132 fls(unsigned mask) 133 { 134 if (mask == 0) 135 return 0; 136 int pos = 1; 137 while (mask != 1) { 138 mask >>= 1; 139 pos++; 140 } 141 return pos; 142 } 143