libubb/: add experimental physical memory allocation/mapping module

The API is fairly inflexible and is very likely to change.

libubb/Makefile

@@ -19,8 +19,9 @@ CFLAGS = -g -Wall -fPIC -Iinclude
 LIB = libubb.a
 SHLIB = libubb.so
 
-OBJS = ubb.o swuart.o mmcclk.o
-HDRS = ubb/ubb.h ubb/regbase.h ubb/regs4740.h ubb/swuart.h ubb/mmcclk.h
+OBJS = ubb.o swuart.o mmcclk.o physmem.o
+HDRS = ubb/ubb.h ubb/regbase.h ubb/regs4740.h ubb/swuart.h ubb/mmcclk.h \
+       ubb/physmem.h
 
 .PHONY:		all clean spotless
 

libubb/physmem.c

@@ -0,0 +1,140 @@
+/*
+ * physmem.c - Physical memory translator and allocator
+ *
+ * Written 2011, 2013 by Werner Almesberger
+ * Copyright 2011, 2013 Werner Almesberger
+ *
+ * This program is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2 of the License, or
+ * (at your option) any later version.
+ */
+
+
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <sys/types.h>
+
+#include <ubb/physmem.h>
+
+
+#define	PAGEMAP_FILE	"/proc/self/pagemap"
+#define	PAGE_SIZE	4096
+
+#define	PM_PSHIFT	55	/* page size */
+#define	PM_PSHIFT_MASK	63
+#define	PM_SWAPPED	(1ULL << 62)
+#define	PM_PRESENT	(1ULL << 63)
+
+#define	ALIGN		32	/* DMA transfer size */
+
+
+static void *align_brk(int alignment)
+{
+	void *initial;
+	unsigned long addr, remainder;
+
+	initial = sbrk(0);
+	addr = (unsigned long) initial;
+	remainder = addr % alignment;
+	if (remainder)
+		sbrk(alignment-remainder);
+	return initial;
+}
+
+
+void *physmem_malloc(size_t size)
+{
+	void *initial;
+	void *buf;
+	unsigned long pos;
+	int err;
+
+	initial = align_brk(ALIGN);
+	pos = (unsigned long) sbrk(0);
+	if ((pos & ~(PAGE_SIZE-1)) != ((pos+size) & ~(PAGE_SIZE-1)))
+		if (sbrk(PAGE_SIZE-(pos & (PAGE_SIZE-1))) == (void *) -1)
+			goto fail;
+	buf = sbrk(size);
+	if (buf != (void *) -1)
+		return buf;
+
+fail:
+	err = errno;
+	brk(initial);
+	errno = err;
+	return NULL;
+}
+
+
+void *physmem_calloc(size_t size)
+{
+	uint8_t *buf;
+
+	buf = physmem_malloc(size);
+	if (buf)
+		memset(buf, 0, size);
+	return buf;
+}
+
+
+static unsigned long xlat_one(int fd, unsigned long vaddr)
+{
+	unsigned long page, offset, paddr;
+	ssize_t got;
+	uint64_t map;
+	int pshift;
+
+	offset = vaddr & (PAGE_SIZE-1);
+	page = vaddr/PAGE_SIZE;
+	if (lseek(fd, page*8, SEEK_SET) < 0) {
+		perror("lseek");
+		exit(1);
+	}
+	got = read(fd, &map, 8);
+	if (got < 0) {
+		perror("read");
+		exit(1);
+	}
+	if (got != 8) {
+		fprintf(stderr, "bad read: got %d bytes instead of 8\n", got);
+		exit(1);
+	}
+	if (!(map & PM_PRESENT)) {
+		fprintf(stderr, "page %lu is not present\n", page);
+		abort();
+	}
+	if (map & PM_SWAPPED) {
+		fprintf(stderr, "page %lu is swapped\n", page);
+		abort();
+	}
+	pshift = (map >> PM_PSHIFT) & PM_PSHIFT_MASK;
+	if ((1 << pshift) != PAGE_SIZE) {
+		fprintf(stderr, "page claims to have size %u\n", 1 << pshift);
+		abort();
+	}
+	paddr = ((map & 0x7fffffffffffffULL) << pshift) | offset;
+//	fprintf(stderr, "0x%lx -> 0x%lx\n", vaddr, paddr);
+	return paddr;
+}
+
+
+unsigned long physmem_xlat(void *v)
+{
+	unsigned long res;
+	int fd;
+
+	fd = open(PAGEMAP_FILE, O_RDONLY);
+	if (fd < 0) {
+		perror(PAGEMAP_FILE);
+		exit(1);
+	}
+	res = xlat_one(fd, (unsigned long) v);
+	close(fd);
+	return res;
+}