Root/libubb/physmem.c

1/*
2 * physmem.c - Physical memory translator and allocator
3 *
4 * Written 2011, 2013 by Werner Almesberger
5 * Copyright 2011, 2013 Werner Almesberger
6 *
7 * This program is free software; you can redistribute it and/or modify
8 * it under the terms of the GNU General Public License as published by
9 * the Free Software Foundation; either version 2 of the License, or
10 * (at your option) any later version.
11 */
12
13
14#define _GNU_SOURCE /* for O_CLOEXEC */
15
16#include <stdint.h>
17#include <stdlib.h>
18#include <stdio.h>
19#include <unistd.h>
20#include <string.h>
21#include <fcntl.h>
22#include <errno.h>
23#include <sys/types.h>
24#include <asm/cachectl.h>
25
26#include <ubb/physmem.h>
27
28
29#define PAGEMAP_FILE "/proc/self/pagemap"
30#define PAGE_SIZE 4096
31
32#define PM_PSHIFT 55 /* page size */
33#define PM_PSHIFT_MASK 63
34#define PM_SWAPPED (1ULL << 62)
35#define PM_PRESENT (1ULL << 63)
36
37#define ALIGN 32 /* DMA transfer size */
38
39
40extern int cacheflush(char *addr, int nbytes, int cache);
41
42
43static void *align_brk(int alignment)
44{
45    void *initial;
46    unsigned long addr, remainder;
47
48    initial = sbrk(0);
49    addr = (unsigned long) initial;
50    remainder = addr % alignment;
51    if (remainder)
52        sbrk(alignment-remainder);
53    return initial;
54}
55
56
57void *physmem_malloc(size_t size)
58{
59    void *initial;
60    void *buf;
61    unsigned long pos;
62    int err;
63
64    initial = align_brk(ALIGN);
65    pos = (unsigned long) sbrk(0);
66    if ((pos & ~(PAGE_SIZE-1)) != ((pos+size) & ~(PAGE_SIZE-1)))
67        if (sbrk(PAGE_SIZE-(pos & (PAGE_SIZE-1))) == (void *) -1)
68            goto fail;
69    buf = sbrk(size);
70    if (buf != (void *) -1)
71        return buf;
72
73fail:
74    err = errno;
75    brk(initial);
76    errno = err;
77    return NULL;
78}
79
80
81void *physmem_calloc(size_t size)
82{
83    uint8_t *buf;
84
85    buf = physmem_malloc(size);
86    if (buf)
87        memset(buf, 0, size);
88    return buf;
89}
90
91
92static unsigned long xlat_one(int fd, unsigned long vaddr)
93{
94    unsigned long page, offset, paddr;
95    ssize_t got;
96    uint64_t map;
97    int pshift;
98
99    offset = vaddr & (PAGE_SIZE-1);
100    page = vaddr/PAGE_SIZE;
101    if (lseek(fd, page*8, SEEK_SET) < 0) {
102        perror("lseek");
103        exit(1);
104    }
105    got = read(fd, &map, 8);
106    if (got < 0) {
107        perror("read");
108        exit(1);
109    }
110    if (got != 8) {
111        fprintf(stderr, "bad read: got %d bytes instead of 8\n", got);
112        exit(1);
113    }
114    if (!(map & PM_PRESENT)) {
115        fprintf(stderr, "page %lu is not present\n", page);
116        abort();
117    }
118    if (map & PM_SWAPPED) {
119        fprintf(stderr, "page %lu is swapped\n", page);
120        abort();
121    }
122    pshift = (map >> PM_PSHIFT) & PM_PSHIFT_MASK;
123    if ((1 << pshift) != PAGE_SIZE) {
124        fprintf(stderr, "page claims to have size %u\n", 1 << pshift);
125        abort();
126    }
127    paddr = ((map & 0x7fffffffffffffULL) << pshift) | offset;
128// fprintf(stderr, "0x%lx -> 0x%lx\n", vaddr, paddr);
129    return paddr;
130}
131
132
133int physmem_xlat(const void *v, size_t len,
134    struct physmem_vec *vec, int vec_len)
135{
136    static int fd = -1;
137    unsigned long vaddr = (unsigned long) v;
138    unsigned long offset, this_len;
139    int n = 0;
140
141    if (fd < 0) {
142        fd = open(PAGEMAP_FILE, O_RDONLY | O_CLOEXEC);
143        if (fd < 0) {
144            perror(PAGEMAP_FILE);
145            exit(1);
146        }
147    }
148
149    while (len) {
150        if (n >= vec_len) {
151            errno = EOVERFLOW;
152            return -1;
153        }
154        offset = vaddr & (PAGE_SIZE-1);
155        this_len = PAGE_SIZE-offset;
156        if (this_len > len)
157            this_len = len;
158        vec->addr = xlat_one(fd, vaddr);
159        vec->len = this_len;
160        vec++;
161        n++;
162        vaddr += this_len;
163        len -= this_len;
164    }
165    return n;
166}
167
168
169int physmem_flush(const void *v, size_t size)
170{
171    if (cacheflush((void *) v, size, DCACHE))
172        return -1;
173    asm("sync"); /* flush the write buffer */
174    return 0;
175}
176

Archive Download this file

Branches:
master



interactive