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 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 32 int trace_get_level(void) 33 { 34 return trace_level; 35 } 36 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 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__) 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 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__) */ 106 static int print_core_id(char *buf __unused, size_t bs __unused) 107 { 108 return 0; 109 } 110 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 */ 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 } 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 209 void trace_set_level(int level __unused) 210 { 211 } 212 213 int trace_get_level(void) 214 { 215 return 0; 216 } 217 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 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 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 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