1#include <stdint.h>
2
3/* A one-bit mask at bit n */
4#define BIT(n) (1ULL << (n))
5
6/* An n-bit mask, beginning at bit 0 */
7#define MASK(n) (BIT(n) - 1)
8
9/* An n-bit field selector, beginning at bit m */
10#define FIELD(m,n,x) (((x) >> m) & MASK(n))
11
12static inline uint64_t
13read_clidr_el1(void) {
14    uint64_t clidr_el1;
15    __asm volatile("mrs %[clidr_el1], clidr_el1" :
16                    [clidr_el1] "=r" (clidr_el1));
17    return clidr_el1;
18}
19
20static inline uint64_t
21read_ccsidr_el1(void) {
22    uint64_t ccsidr_el1;
23    __asm volatile("mrs %[ccsidr_el1], ccsidr_el1" :
24                    [ccsidr_el1] "=r" (ccsidr_el1));
25    return ccsidr_el1;
26}
27
28static inline void
29write_csselr_el1(int level, int instruction) {
30    /* Register format:
31     * 31 4 | 3   1 | 0
32     * RES0 | level | instruction
33     */
34    uint64_t x= (instruction & 0x1) | ((level & 0x7) << 1);
35    __asm volatile("msr csselr_el1, %0" : : "r" (x));
36}
37
38#define CTYPE(n,x) FIELD(3 * (n-1), 3 * (n-1) + 2, x)
39
40enum armv8_cache_type {
41    ARMv8_CACHE_NONE    = 0,
42    ARMv8_CACHE_IONLY   = 1,
43    ARMv8_CACHE_DONLY   = 2,
44    ARMv8_CACHE_ID      = 3,
45    ARMv8_CACHE_UNIFIED = 4,
46};
47
48static inline int
49clz(uint64_t x) {
50    int r;
51    __asm volatile("clz %[r], %[x]" : [r] "=r"(r) : [x] "r"(x));
52    return r;
53}
54
55static inline int
56log2i(uint64_t x) {
57    return 64 - clz(x-1);
58}
59
60void
61invalidate_caches(void) {
62    uint64_t clidr= read_clidr_el1();
63    int loc= FIELD(24, 26, clidr);
64
65    /* Invalidate all instruction caches to point of unification. */
66    __asm volatile("ic iallu");
67
68    /* Invalidate all data caches up to the point of coherence. */
69    for(int level= 1; level <= loc; level++) {
70        int ctype= CTYPE(level, clidr);
71
72        /* Only worry about levels with a data cache. */
73        if(ctype == ARMv8_CACHE_DONLY ||
74           ctype == ARMv8_CACHE_ID ||
75           ctype == ARMv8_CACHE_UNIFIED) {
76            /* Read the data cache size & associativity. */
77            write_csselr_el1(level-1, 0);
78            uint64_t ccsidr= read_ccsidr_el1();
79            int sets=     FIELD(13, 15, ccsidr) + 1,
80                assoc=    FIELD( 3, 10, ccsidr) + 1,
81                linebits= FIELD( 0,  3, ccsidr) + 4;
82
83            /* Calculate the field offsets for the invalidate operation. */
84            int setbits=   log2i(sets),
85                assocbits= log2i(assoc);
86
87            for(int w= 0; w < assoc; w++) {
88                for(int s= 0; s < sets; s++) {
89                    uint64_t op=
90                        ((w & MASK(assocbits)) << (32-assocbits)) |
91                        ((s & MASK(setbits)) << linebits) |
92                        ((level & MASK(3)) << 1);
93                    __asm volatile("dc isw, %[op]" : : [op] "r" (op));
94                }
95            }
96        }
97    }
98}
99