1 // SPDX-License-Identifier: BSD-2-Clause
2 /*
3 * Copyright (c) 2014, STMicroelectronics International N.V.
4 */
5
6 #if defined(__KERNEL__)
7 #include <platform_config.h>
8 #endif
9
10 #include <config.h>
11 #include <printk.h>
12 #include <stdarg.h>
13 #include <string.h>
14 #include <trace.h>
15 #include <types_ext.h>
16 #include <util.h>
17
18 #if (TRACE_LEVEL < TRACE_MIN) || (TRACE_LEVEL > TRACE_MAX)
19 #error "Invalid value of TRACE_LEVEL"
20 #endif
21
22 #if (TRACE_LEVEL >= TRACE_ERROR)
23
trace_set_level(int level)24 void trace_set_level(int level)
25 {
26 if (((int)level >= TRACE_MIN) && (level <= TRACE_MAX))
27 trace_level = level;
28 else
29 trace_level = TRACE_MAX;
30 }
31
trace_get_level(void)32 int trace_get_level(void)
33 {
34 return trace_level;
35 }
36
trace_level_to_string(int level,bool level_ok)37 static char trace_level_to_string(int level, bool level_ok)
38 {
39 /*
40 * U = Unused
41 * E = Error
42 * I = Information
43 * D = Debug
44 * F = Flow
45 */
46 static const char lvl_strs[] = { 'U', 'E', 'I', 'D', 'F' };
47 int l = 0;
48
49 if (!level_ok)
50 return 'M';
51
52 if ((level >= TRACE_MIN) && (level <= TRACE_MAX))
53 l = level;
54
55 return lvl_strs[l];
56 }
57
print_thread_id(char * buf,size_t bs)58 static int print_thread_id(char *buf, size_t bs)
59 {
60 #if CFG_NUM_THREADS > 100
61 int num_thread_digits = 3;
62 #elif CFG_NUM_THREADS > 10
63 int num_thread_digits = 2;
64 #else
65 int num_thread_digits = 1;
66 #endif
67 int thread_id = trace_ext_get_thread_id();
68
69 if (thread_id >= 0)
70 return snprintk(buf, bs, "%0*d ", num_thread_digits, thread_id);
71 else
72 return snprintk(buf, bs, "%*s ", num_thread_digits, "");
73 }
74
75 #if defined(__KERNEL__)
print_core_id(char * buf,size_t bs)76 static int print_core_id(char *buf, size_t bs)
77 {
78 #if CFG_TEE_CORE_NB_CORE > 100
79 const int num_digits = 3;
80 const char qm[] = "???";
81 #elif CFG_TEE_CORE_NB_CORE > 10
82 const int num_digits = 2;
83 const char qm[] = "??";
84 #else
85 const int num_digits = 1;
86 const char qm[] = "?";
87 #endif
88 int core_id = trace_ext_get_core_id();
89
90 if (core_id >= 0)
91 return snprintk(buf, bs, "%0*u ", num_digits, core_id);
92 else
93 return snprintk(buf, bs, "%s ", qm);
94 }
95
print_guest_id(char * buf,size_t bs)96 static int print_guest_id(char *buf, size_t bs)
97 {
98 if (IS_ENABLED(CFG_NS_VIRTUALIZATION))
99 return snprintk(buf, bs, "%d ", trace_ext_get_guest_id());
100 else
101 return 0;
102
103
104 }
105 #else /* defined(__KERNEL__) */
print_core_id(char * buf __unused,size_t bs __unused)106 static int print_core_id(char *buf __unused, size_t bs __unused)
107 {
108 return 0;
109 }
110
print_guest_id(char * buf __unused,size_t bs __unused)111 static int print_guest_id(char *buf __unused, size_t bs __unused)
112 {
113 return 0;
114 }
115 #endif
116
117 /* Format trace of user ta. Inline with kernel ta */
trace_printf(const char * function,int line,int level,bool level_ok,const char * fmt,...)118 void trace_printf(const char *function, int line, int level, bool level_ok,
119 const char *fmt, ...)
120 {
121 va_list ap;
122
123 va_start(ap, fmt);
124 trace_vprintf(function, line, level, level_ok, fmt, ap);
125 va_end(ap);
126 }
trace_vprintf(const char * function,int line,int level,bool level_ok,const char * fmt,va_list ap)127 void trace_vprintf(const char *function, int line, int level, bool level_ok,
128 const char *fmt, va_list ap)
129 {
130 char buf[MAX_PRINT_SIZE];
131 size_t boffs = 0;
132 int res;
133
134 if (level_ok && level > trace_level)
135 return;
136
137 /* Print the type of message */
138 res = snprintk(buf, sizeof(buf), "%c/",
139 trace_level_to_string(level, level_ok));
140 if (res < 0)
141 return;
142 boffs += res;
143
144 /* Print the location, i.e., TEE core or TA */
145 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:",
146 trace_ext_prefix);
147 if (res < 0)
148 return;
149 boffs += res;
150
151 if (level_ok && (BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) {
152 /* Print the current guest ID or 0 for Nexus */
153 res = print_guest_id(buf + boffs, sizeof(buf) - boffs);
154 if (res < 0)
155 return;
156 boffs += res;
157
158 /* Print the core ID if in atomic context */
159 res = print_core_id(buf + boffs, sizeof(buf) - boffs);
160 if (res < 0)
161 return;
162 boffs += res;
163
164 /* Print the Thread ID */
165 res = print_thread_id(buf + boffs, sizeof(buf) - boffs);
166 if (res < 0)
167 return;
168 boffs += res;
169
170 if (function) {
171 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:%d ",
172 function, line);
173 if (res < 0)
174 return;
175 boffs += res;
176 }
177 } else {
178 /* Add space after location info */
179 if (boffs >= sizeof(buf) - 1)
180 return;
181 buf[boffs++] = ' ';
182 buf[boffs] = 0;
183 }
184
185 res = vsnprintk(buf + boffs, sizeof(buf) - boffs, fmt, ap);
186 if (res > 0)
187 boffs += res;
188
189 if (boffs >= (sizeof(buf) - 1))
190 boffs = sizeof(buf) - 2;
191
192 buf[boffs] = '\n';
193 while (boffs && buf[boffs] == '\n')
194 boffs--;
195 boffs++;
196 buf[boffs + 1] = '\0';
197
198 trace_ext_puts(buf);
199 }
200
201 #else
202
203 /*
204 * In case we have a zero or negative trace level when compiling optee_os, we
205 * have to add stubs to trace functions in case they are used with TA having a
206 * non-zero trace level
207 */
208
trace_set_level(int level __unused)209 void trace_set_level(int level __unused)
210 {
211 }
212
trace_get_level(void)213 int trace_get_level(void)
214 {
215 return 0;
216 }
217
trace_printf(const char * function __unused,int line __unused,int level __unused,bool level_ok __unused,const char * fmt __unused,...)218 void trace_printf(const char *function __unused, int line __unused,
219 int level __unused, bool level_ok __unused,
220 const char *fmt __unused, ...)
221 {
222 }
223
224 #endif
225
226 #if (TRACE_LEVEL >= TRACE_DEBUG)
227 struct strbuf {
228 char buf[MAX_PRINT_SIZE];
229 char *ptr;
230 };
231
append(struct strbuf * sbuf,const char * fmt,...)232 static int __printf(2, 3) append(struct strbuf *sbuf, const char *fmt, ...)
233 {
234 int left;
235 int len;
236 va_list ap;
237
238 if (sbuf->ptr == NULL)
239 sbuf->ptr = sbuf->buf;
240 left = sizeof(sbuf->buf) - (sbuf->ptr - sbuf->buf);
241 va_start(ap, fmt);
242 len = vsnprintk(sbuf->ptr, left, fmt, ap);
243 va_end(ap);
244 if (len < 0) {
245 /* Format error */
246 return 0;
247 }
248 if (len >= left) {
249 /* Output was truncated */
250 return 0;
251 }
252 sbuf->ptr += MIN(left, len);
253 return 1;
254 }
255
dhex_dump(const char * function,int line,int level,const void * buf,int len)256 void dhex_dump(const char *function, int line, int level,
257 const void *buf, int len)
258 {
259 int i;
260 int ok;
261 struct strbuf sbuf;
262 char *in = (char *)buf;
263
264 if (level <= trace_level) {
265 sbuf.ptr = NULL;
266 for (i = 0; i < len; i++) {
267 if ((i % 16) == 0) {
268 ok = append(&sbuf, "%0*" PRIxVA " ",
269 PRIxVA_WIDTH, (vaddr_t)(in + i));
270 if (!ok)
271 goto err;
272 }
273 ok = append(&sbuf, "%02x ", in[i]);
274 if (!ok)
275 goto err;
276 if ((i % 16) == 7) {
277 ok = append(&sbuf, " ");
278 if (!ok)
279 goto err;
280 } else if ((i % 16) == 15) {
281 trace_printf(function, line, level, true, "%s",
282 sbuf.buf);
283 sbuf.ptr = NULL;
284 }
285 }
286 if (sbuf.ptr) {
287 /* Buffer is not empty: flush it */
288 trace_printf(function, line, level, true, "%s",
289 sbuf.buf);
290
291 }
292 }
293 return;
294 err:
295 DMSG("Hex dump error");
296 }
297 #else
298
299 /*
300 * In case we have trace level less than debug when compiling optee_os, we have
301 * to add stubs to trace functions in case they are used with TA having a
302 * a higher trace level
303 */
304
dhex_dump(const char * function __unused,int line __unused,int level __unused,const void * buf __unused,int len __unused)305 void dhex_dump(const char *function __unused, int line __unused,
306 int level __unused,
307 const void *buf __unused, int len __unused)
308 {
309 }
310
311 #endif
312