+extern "C" void
+iopa_init(iopa_t * a)
+{
+ bzero(a, sizeof(*a));
+ a->lock = IOLockAlloc();
+ queue_init(&a->list);
+}
+
+static uintptr_t
+iopa_allocinpage(iopa_page_t * pa, uint32_t count, uint64_t align)
+{
+ uint32_t n, s;
+ uint64_t avail = pa->avail;
+
+ assert(avail);
+
+ // find strings of count 1 bits in avail
+ for (n = count; n > 1; n -= s)
+ {
+ s = n >> 1;
+ avail = avail & (avail << s);
+ }
+ // and aligned
+ avail &= align;
+
+ if (avail)
+ {
+ n = __builtin_clzll(avail);
+ pa->avail &= ~((-1ULL << (64 - count)) >> n);
+ if (!pa->avail && pa->link.next)
+ {
+ remque(&pa->link);
+ pa->link.next = 0;
+ }
+ return (n * gIOPageAllocChunkBytes + trunc_page((uintptr_t) pa));
+ }
+
+ return (0);
+}
+
+uintptr_t
+iopa_alloc(iopa_t * a, iopa_proc_t alloc, vm_size_t bytes, uint32_t balign)
+{
+ static const uint64_t align_masks[] = {
+ 0xFFFFFFFFFFFFFFFF,
+ 0xAAAAAAAAAAAAAAAA,
+ 0x8888888888888888,
+ 0x8080808080808080,
+ 0x8000800080008000,
+ 0x8000000080000000,
+ 0x8000000000000000,
+ };
+ iopa_page_t * pa;
+ uintptr_t addr = 0;
+ uint32_t count;
+ uint64_t align;
+
+ if (!bytes) bytes = 1;
+ count = (bytes + gIOPageAllocChunkBytes - 1) / gIOPageAllocChunkBytes;
+ align = align_masks[log2up((balign + gIOPageAllocChunkBytes - 1) / gIOPageAllocChunkBytes)];
+
+ IOLockLock(a->lock);
+ __IGNORE_WCASTALIGN(pa = (typeof(pa)) queue_first(&a->list));
+ while (!queue_end(&a->list, &pa->link))
+ {
+ addr = iopa_allocinpage(pa, count, align);
+ if (addr)
+ {
+ a->bytecount += bytes;
+ break;
+ }
+ __IGNORE_WCASTALIGN(pa = (typeof(pa)) queue_next(&pa->link));
+ }
+ IOLockUnlock(a->lock);
+
+ if (!addr)
+ {
+ addr = alloc(a);
+ if (addr)
+ {
+ pa = (typeof(pa)) (addr + page_size - gIOPageAllocChunkBytes);
+ pa->signature = kIOPageAllocSignature;
+ pa->avail = -2ULL;
+
+ addr = iopa_allocinpage(pa, count, align);
+ IOLockLock(a->lock);
+ if (pa->avail) enqueue_head(&a->list, &pa->link);
+ a->pagecount++;
+ if (addr) a->bytecount += bytes;
+ IOLockUnlock(a->lock);
+ }
+ }
+
+ assert((addr & ((1 << log2up(balign)) - 1)) == 0);
+ return (addr);
+}
+
+uintptr_t
+iopa_free(iopa_t * a, uintptr_t addr, vm_size_t bytes)
+{
+ iopa_page_t * pa;
+ uint32_t count;
+ uintptr_t chunk;
+
+ if (!bytes) bytes = 1;
+
+ chunk = (addr & page_mask);
+ assert(0 == (chunk & (gIOPageAllocChunkBytes - 1)));
+
+ pa = (typeof(pa)) (addr | (page_size - gIOPageAllocChunkBytes));
+ assert(kIOPageAllocSignature == pa->signature);
+
+ count = (bytes + gIOPageAllocChunkBytes - 1) / gIOPageAllocChunkBytes;
+ chunk /= gIOPageAllocChunkBytes;
+
+ IOLockLock(a->lock);
+ if (!pa->avail)
+ {
+ assert(!pa->link.next);
+ enqueue_tail(&a->list, &pa->link);
+ }
+ pa->avail |= ((-1ULL << (64 - count)) >> chunk);
+ if (pa->avail != -2ULL) pa = 0;
+ else
+ {
+ remque(&pa->link);
+ pa->link.next = 0;
+ pa->signature = 0;
+ a->pagecount--;
+ // page to free
+ pa = (typeof(pa)) trunc_page(pa);
+ }
+ a->bytecount -= bytes;
+ IOLockUnlock(a->lock);
+
+ return ((uintptr_t) pa);
+}
+
+/* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
+
projects / apple / xnu.git / blobdiff