From 6cd21404cdb88a478a6e8365420de5013013cbb5 Mon Sep 17 00:00:00 2001
From: Werner Almesberger <werner@openmoko.org>
Date: Fri, 29 Apr 2011 21:15:27 -0300
Subject: [PATCH] physmem.c: added virtual to physical translation

- physmem.c (calloc_phys_vec): malloc the vector instead of sbrk'ing it
- ubb-vga.h (xlat_virt), physmem.c (xlat_one, xlat_virt): translate a
  vector of virtual addresses to physical addresses
---
 ubb-vga/physmem.c | 87 +++++++++++++++++++++++++++++++++++++++++++----
 ubb-vga/ubb-vga.h |  1 +
 2 files changed, 82 insertions(+), 6 deletions(-)

diff --git a/ubb-vga/physmem.c b/ubb-vga/physmem.c
index 348dda0..af9fb9f 100644
--- a/ubb-vga/physmem.c
+++ b/ubb-vga/physmem.c
@@ -11,14 +11,22 @@
  */
 
 
-#include <string.h>
+#include <stdint.h>
+#include <stdlib.h>
+#include <stdio.h>
 #include <unistd.h>
+#include <string.h>
+#include <fcntl.h>
 #include <sys/types.h>
 
 
-/*
- * dummy implementation
- */
+#define	PAGEMAP_FILE	"/proc/self/pagemap"
+#define	PAGE_SIZE	4096
+
+#define	PM_PSHIFT	55	/* page size */
+#define	PM_PSHIFT_MASK	63
+#define	PM_SWAPPED	62
+#define	PM_PRESENT	63
 
 
 static void align_brk(int alignment)
@@ -35,8 +43,11 @@ void **calloc_phys_vec(size_t n, size_t size)
 	void **vec;
 	int i;
 
-	align_brk(sizeof(void *));
-	vec = sbrk(sizeof(void *)*n);
+	vec = malloc(sizeof(void *)*n);
+	if (!vec) {
+		perror("malloc");
+		exit(1);
+	}
 
 	for (i = 0; i != n; i++) {
 		align_brk(512);	/* crude page alignment */
@@ -45,3 +56,67 @@ void **calloc_phys_vec(size_t n, size_t size)
 	}
 	return vec;
 }
+
+
+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 instead of 8\n", got);
+		exit(1);
+	}
+	if (!((map >> PM_PRESENT) & 1)) {
+		fprintf(stderr, "page %lu is not present\n", page);
+		abort();
+	}
+	if ((map >> PM_SWAPPED) & 1) {
+		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%x -> 0x%x\n", vaddr, paddr);
+	return paddr;
+}
+
+
+unsigned long *xlat_virt(void *const *v, size_t n)
+{
+	unsigned long *res;
+	int fd;
+	int i;
+
+	res = malloc(n*sizeof(unsigned long));
+	if (!res) {
+		perror("malloc");
+		exit(1);
+	}
+	fd = open(PAGEMAP_FILE, O_RDONLY);
+	if (fd < 0) {
+		perror(PAGEMAP_FILE);
+		exit(1);
+	}
+	for (i = 0; i != n; i++)
+		res[i] = xlat_one(fd, (unsigned long) v[i]);
+	close(fd);
+	return res;
+}
diff --git a/ubb-vga/ubb-vga.h b/ubb-vga/ubb-vga.h
index fb5cf57..67b7db3 100644
--- a/ubb-vga/ubb-vga.h
+++ b/ubb-vga/ubb-vga.h
@@ -34,6 +34,7 @@ void ccube_init(void);
 /* physmem.c */
 
 void **calloc_phys_vec(size_t n, size_t size);
+unsigned long *xlat_virt(void *const *v, size_t n);
 
 /* grabfb.c */