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
17static inline uint32
18round_to_pagesize(uint32 size)
19{
20	return (size + B_PAGE_SIZE - 1) & ~(B_PAGE_SIZE - 1);
21}
22
23
24area_id
25alloc_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
58area_id
59map_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
92status_t
93sg_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
119void
120swap_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
131int
132fls(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