src/add-ons/kernel/busses/scsi/ahci/util.cpp

   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         int i;
  97         for (i = 0; i < sgCount && dataSize > 0; i++) {
  98                 size_t size = min_c(dataSize, sgTable[i].size);
  99
 100                 TRACE("sg_memcpy phyAddr %#" B_PRIxPHYSADDR ", size %lu\n",
 101                         sgTable[i].address, size);
 102
 103                 vm_memcpy_to_physical(sgTable[i].address, data, size, false);
 104
 105                 data = (char *)data + size;
 106                 dataSize -= size;
 107         }
 108         if (dataSize != 0)
 109                 return B_ERROR;
 110         return B_OK;
 111 }
 112
 113
 114 void
 115 swap_words(void *data, size_t size)
 116 {
 117         uint16 *word = (uint16*)data;
 118         size_t count = size / 2;
 119         while (count--) {
 120                 *word = (*word << 8) | (*word >> 8);
 121                 word++;
 122         }
 123 }
 124
 125
 126 int
 127 fls(unsigned mask)
 128 {
 129         if (mask == 0)
 130                 return 0;
 131         int pos = 1;
 132         while (mask != 1) {
 133                 mask >>= 1;
 134                 pos++;
 135         }
 136         return pos;
 137 }