mirror of
git://projects.qi-hardware.com/ben-blinkenlights.git
synced 2024-11-27 19:30:36 +02:00
141 lines
2.7 KiB
C
141 lines
2.7 KiB
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;
|
|
}
|