1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2022, Linaro Limited
4 */
5
6 #include <arm_user_sysreg.h>
7 #include <assert.h>
8 #include <config.h>
9 #include <memtag.h>
10 #include <string.h>
11
12 #if MEMTAG_IS_ENABLED
13
14 /* This is for AArch64 only, MTE is only available in this mode */
15
dczid_block_size(void)16 static unsigned int dczid_block_size(void)
17 {
18 return SHIFT_U32(4U, read_dczid_el0() & 0xf);
19 }
20
data_zero_prohibited(void)21 static bool data_zero_prohibited(void)
22 {
23 #ifdef __KERNEL__
24 return false;
25 #else
26 return read_dczid_el0() & BIT(4);
27 #endif
28 }
29
dc_gzva(uint64_t va)30 static void dc_gzva(uint64_t va)
31 {
32 asm volatile ("dc gzva, %0" : : "r" (va));
33 }
34
dc_gva(uint64_t va)35 static void dc_gva(uint64_t va)
36 {
37 asm volatile ("dc gva, %0" : : "r" (va));
38 }
39
stg_and_advance(vaddr_t va)40 static vaddr_t stg_and_advance(vaddr_t va)
41 {
42 asm volatile("stg %0, [%0], #16" : "+r"(va) : : "memory");
43 return va;
44 }
45
insert_random_tag(void * addr)46 static void *insert_random_tag(void *addr)
47 {
48 asm volatile("irg %0, %0" : "+r"(addr) : : );
49 return addr;
50 }
51
load_tag(void * addr)52 static void *load_tag(void *addr)
53 {
54 asm volatile("ldg %0, [%0]" : "+r"(addr) : : );
55 return addr;
56 }
57
set_tags_dc_gva(vaddr_t va,size_t size,size_t dcsz)58 static void set_tags_dc_gva(vaddr_t va, size_t size, size_t dcsz)
59 {
60 do {
61 dc_gva(va);
62 va += dcsz;
63 size -= dcsz;
64 } while (size);
65 }
66
clear_mem_dc_gzva(vaddr_t va,size_t size,size_t dcsz)67 static void clear_mem_dc_gzva(vaddr_t va, size_t size, size_t dcsz)
68 {
69 do {
70 dc_gzva(va);
71 va += dcsz;
72 size -= dcsz;
73 } while (size);
74 }
75
set_tags_helper(void * addr,size_t size)76 static void *set_tags_helper(void *addr, size_t size)
77 {
78 vaddr_t va = (vaddr_t)addr;
79 vaddr_t end = va + size;
80
81 assert(!(va & MEMTAG_GRANULE_MASK));
82 assert(!(size & MEMTAG_GRANULE_MASK));
83
84 while (va < end)
85 va = stg_and_advance(va);
86
87 return addr;
88 }
89
set_tags_dc_helper(void * addr,size_t size)90 static void *set_tags_dc_helper(void *addr, size_t size)
91 {
92 size_t dcsz = dczid_block_size();
93 vaddr_t va = (vaddr_t)addr;
94 size_t mask = dcsz - 1;
95 size_t s = 0;
96
97 if (va & mask) {
98 s = MIN(dcsz - (va & mask), size);
99 set_tags_helper((void *)va, s);
100 va += s;
101 size -= s;
102 }
103 s = size & ~mask;
104 if (s) {
105 set_tags_dc_gva(va, s, dcsz);
106 va += s;
107 size -= s;
108 }
109 if (size)
110 set_tags_helper((void *)va, size);
111
112 return addr;
113 }
114
set_tags_dc(void * addr,size_t size,uint8_t tag)115 static void *set_tags_dc(void *addr, size_t size, uint8_t tag)
116 {
117 return set_tags_dc_helper(memtag_insert_tag(addr, tag), size);
118 }
119
set_random_tags_dc(void * addr,size_t size)120 static void *set_random_tags_dc(void *addr, size_t size)
121 {
122 return set_tags_dc_helper(insert_random_tag(addr), size);
123 }
124
set_tags(void * addr,size_t size,uint8_t tag)125 static void *set_tags(void *addr, size_t size, uint8_t tag)
126 {
127 return set_tags_helper(memtag_insert_tag(addr, tag), size);
128 }
129
set_random_tags(void * addr,size_t size)130 static void *set_random_tags(void *addr, size_t size)
131 {
132 return set_tags_helper(insert_random_tag(addr), size);
133 }
134
clear_mem(void * va,size_t size)135 static void clear_mem(void *va, size_t size)
136 {
137 set_tags(va, size, 0);
138 memset(memtag_strip_tag(va), 0, size);
139 }
140
clear_mem_dc(void * addr,size_t size)141 static void clear_mem_dc(void *addr, size_t size)
142 {
143 size_t dcsz = dczid_block_size();
144 vaddr_t va = (vaddr_t)addr;
145 size_t mask = dcsz - 1;
146 size_t s = 0;
147
148 if (va & mask) {
149 s = MIN(dcsz - (va & mask), size);
150 clear_mem((void *)va, s);
151 va += s;
152 size -= s;
153 }
154 s = size & ~mask;
155 if (s) {
156 clear_mem_dc_gzva(va, s, dcsz);
157 va += s;
158 size -= s;
159 }
160 if (size)
161 clear_mem((void *)va, size);
162 }
163
read_tag(const void * addr)164 static uint8_t read_tag(const void *addr)
165 {
166 return memtag_get_tag(load_tag((void *)addr));
167 }
168
169 static const struct __memtag_ops ops_dc_enabled = {
170 .set_tags = set_tags_dc,
171 .set_random_tags = set_random_tags_dc,
172 .clear_mem = clear_mem_dc,
173 .read_tag = read_tag,
174 };
175
176 static const struct __memtag_ops ops_enabled = {
177 .set_tags = set_tags,
178 .set_random_tags = set_random_tags,
179 .clear_mem = clear_mem,
180 .read_tag = read_tag,
181 };
182
183 const struct __memtag_ops __memtag_ops_disabled = {
184 .set_tags = __memtag_disabled_set_tags,
185 .set_random_tags = __memtag_disabled_set_random_tags,
186 .clear_mem = __memtag_disabled_clear_mem,
187 .read_tag = __memtag_disabled_read_tag,
188 };
189
190 const struct __memtag_ops *__memtag_ops = &__memtag_ops_disabled;
191
memtag_init_ops(unsigned int memtag_impl)192 void memtag_init_ops(unsigned int memtag_impl)
193 {
194 if (memtag_impl >= 2) {
195 /*
196 * Data zero is always available for S-EL1 if MTE is
197 * enabled, but for S-EL0 it may depend on configuration.
198 */
199 if (data_zero_prohibited())
200 __memtag_ops = &ops_enabled;
201 else
202 __memtag_ops = &ops_dc_enabled;
203 } else {
204 __memtag_ops = &__memtag_ops_disabled;
205 }
206 }
207 #endif
208