1/*
2 * Copyright 2014, General Dynamics C4 Systems
3 *
4 * This software may be distributed and modified according to the terms of
5 * the GNU General Public License version 2. Note that NO WARRANTY is provided.
6 * See "LICENSE_GPLv2.txt" for details.
7 *
8 * @TAG(GD_GPL)
9 */
10
11#include <arch/machine/hardware.h>
12
13static inline void invalidateByWSL(word_t wsl)
14{
15    asm volatile("mcr p15, 0, %0, c7, c6, 2" : : "r"(wsl));
16}
17
18static inline void cleanByWSL(word_t wsl)
19{
20    asm volatile("mcr p15, 0, %0, c7, c10, 2" : : "r"(wsl));
21}
22
23static inline void cleanInvalidateByWSL(word_t wsl)
24{
25    asm volatile("mcr p15, 0, %0, c7, c14, 2" : : "r"(wsl));
26}
27
28
29static inline word_t readCLID(void)
30{
31    word_t CLID;
32    asm volatile("mrc p15, 1, %0, c0, c0, 1" : "=r"(CLID));
33    return CLID;
34}
35
36#define LOUU(x)    (((x) >> 27)        & MASK(3))
37#define LOC(x)     (((x) >> 24)        & MASK(3))
38#define LOUIS(x)   (((x) >> 21)        & MASK(3))
39#define CTYPE(x,n) (((x) >> (n*3))     & MASK(3))
40
41enum arm_cache_type {
42    ARMCacheNone = 0,
43    ARMCacheI =    1,
44    ARMCacheD =    2,
45    ARMCacheID =   3,
46    ARMCacheU =    4,
47};
48
49
50static inline word_t readCacheSize(int level, bool_t instruction)
51{
52    word_t size_unique_name, csselr_old;
53    /* Save CSSELR */
54    asm volatile("mrc p15, 2, %0, c0, c0, 0" : "=r"(csselr_old));
55    /* Select cache level */
56    asm volatile("mcr p15, 2, %0, c0, c0, 0" : : "r"((level << 1) | instruction));
57    /* Read 'size' */
58    asm volatile("mrc p15, 1, %0, c0, c0, 0" : "=r"(size_unique_name));
59    /* Restore CSSELR */
60    asm volatile("mcr p15, 2, %0, c0, c0, 0" : : "r"(csselr_old));
61    return size_unique_name;
62}
63
64/* Number of bits to index within a cache line.  The field is log2(nwords) - 2
65 * , and thus by adding 4 we get log2(nbytes). */
66#define LINEBITS(s) (( (s)        & MASK(3))  + 4)
67/* Associativity, field is assoc - 1. */
68#define ASSOC(s)    ((((s) >> 3)  & MASK(10)) + 1)
69/* Number of sets, field is nsets - 1. */
70#define NSETS(s)    ((((s) >> 13) & MASK(15)) + 1)
71
72
73void
74clean_D_PoU(void)
75{
76    int clid = readCLID();
77    int lou = LOUU(clid);
78    int l;
79
80    for (l = 0; l < lou; l++) {
81        if (CTYPE(clid, l) > ARMCacheI) {
82            word_t s = readCacheSize(l, 0);
83            int lbits = LINEBITS(s);
84            int assoc = ASSOC(s);
85            int assoc_bits = wordBits - clzl(assoc - 1);
86            int nsets = NSETS(s);
87            int w;
88
89            for (w = 0; w < assoc; w++) {
90                int v;
91
92                for (v = 0; v < nsets; v++) {
93                    cleanByWSL((w << (32 - assoc_bits)) |
94                               (v << lbits) | (l << 1));
95                }
96            }
97        }
98    }
99}
100
101
102void
103cleanInvalidate_D_PoC(void)
104{
105    int clid = readCLID();
106    int loc = LOC(clid);
107    int l;
108
109    for (l = 0; l < loc; l++) {
110        if (CTYPE(clid, l) > ARMCacheI) {
111            word_t s = readCacheSize(l, 0);
112            int lbits = LINEBITS(s);
113            int assoc = ASSOC(s);
114            int assoc_bits = wordBits - clzl(assoc - 1);
115            int nsets = NSETS(s);
116            int w;
117
118            for (w = 0; w < assoc; w++) {
119                int v;
120
121                for (v = 0; v < nsets; v++) {
122                    cleanInvalidateByWSL((w << (32 - assoc_bits)) |
123                                         (v << lbits) | (l << 1));
124                }
125            }
126        }
127    }
128}
129