ben-blinkenlights/libubb/physmem.c

/*
 * 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;
}