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.
 */


#define	_GNU_SOURCE	/* for O_CLOEXEC */

#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 <asm/cachectl.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 */


extern int cacheflush(char *addr, int nbytes, int cache);


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


int physmem_xlat(void *v, size_t len, struct physmem_vec *vec, int vec_len)
{
	static int fd = -1;
	unsigned long vaddr = (unsigned long) v;
	unsigned long offset, this_len;
	int n = 0;

	if (fd < 0) {
		fd = open(PAGEMAP_FILE, O_RDONLY | O_CLOEXEC);
		if (fd < 0) {
			perror(PAGEMAP_FILE);
			exit(1);
		}
	}

	while (len) {
		if (n >= vec_len) {
			close(fd);
			errno = EOVERFLOW;
			return -1;
		}
		offset = vaddr & (PAGE_SIZE-1);
		this_len = PAGE_SIZE-offset;
		if (this_len > len)
			this_len = len;
		vec->addr = xlat_one(fd, vaddr);
		vec->len = this_len;
		vec++;
		n++;
		vaddr += this_len;
		len -= this_len;
	}
	close(fd);
	return n;
}


int physmem_flush(const void *v, size_t size)
{
	if (cacheflush((void *) v, size, DCACHE))
		return -1;
	asm("sync");	/* flush the write buffer */
	return 0;
}