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 #include <kernel/misc.h> 9 #endif 10 11 #include <printk.h> 12 #include <stdarg.h> 13 #include <string.h> 14 #include <trace.h> 15 #include <util.h> 16 #include <types_ext.h> 17 18 #if (TRACE_LEVEL > 0) 19 20 #if (TRACE_LEVEL < TRACE_MIN) || (TRACE_LEVEL > TRACE_MAX) 21 #error "Invalid value of TRACE_LEVEL" 22 #endif 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, int thread_id) 59 { 60 #if CFG_NUM_THREADS > 9 61 int num_thread_digits = 2; 62 #else 63 int num_thread_digits = 1; 64 #endif 65 66 if (thread_id >= 0) 67 return snprintk(buf, bs, "%0*d ", num_thread_digits, thread_id); 68 else 69 return snprintk(buf, bs, "%*s ", num_thread_digits, ""); 70 } 71 72 #if defined(__KERNEL__) 73 static int print_core_id(char *buf, size_t bs) 74 { 75 #if CFG_TEE_CORE_NB_CORE > 9 76 const int num_digits = 2; 77 #else 78 const int num_digits = 1; 79 #endif 80 81 if (thread_get_exceptions() & THREAD_EXCP_FOREIGN_INTR) 82 return snprintk(buf, bs, "%0*zu ", num_digits, get_core_pos()); 83 else 84 return snprintk(buf, bs, "%*s ", num_digits, "?"); 85 } 86 #else /* defined(__KERNEL__) */ 87 static int print_core_id(char *buf __unused, size_t bs __unused) 88 { 89 return 0; 90 } 91 #endif 92 93 /* Format trace of user ta. Inline with kernel ta */ 94 void trace_printf(const char *function, int line, int level, bool level_ok, 95 const char *fmt, ...) 96 { 97 va_list ap; 98 char buf[MAX_PRINT_SIZE]; 99 size_t boffs = 0; 100 int res; 101 int thread_id; 102 103 if (level_ok && level > trace_level) 104 return; 105 106 /* Print the type of message */ 107 res = snprintk(buf, sizeof(buf), "%c/", 108 trace_level_to_string(level, level_ok)); 109 if (res < 0) 110 return; 111 boffs += res; 112 113 /* Print the location, i.e., TEE core or TA */ 114 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:", 115 trace_ext_prefix); 116 if (res < 0) 117 return; 118 boffs += res; 119 120 /* Print the core ID if in atomic context */ 121 if (level_ok && (BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 122 res = print_core_id(buf + boffs, sizeof(buf) - boffs); 123 else 124 res = 0; 125 if (res < 0) 126 return; 127 boffs += res; 128 129 /* Print the Thread ID */ 130 if (level_ok && !(BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 131 thread_id = -1; 132 else 133 thread_id = trace_ext_get_thread_id(); 134 135 res = print_thread_id(buf + boffs, sizeof(buf) - boffs, thread_id); 136 137 if (res < 0) 138 return; 139 boffs += res; 140 141 /* Print the function and line */ 142 if (level_ok && !(BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 143 function = NULL; 144 145 if (function) { 146 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:%d ", 147 function, line); 148 if (res < 0) 149 return; 150 boffs += res; 151 } 152 153 va_start(ap, fmt); 154 res = vsnprintk(buf + boffs, sizeof(buf) - boffs, fmt, ap); 155 va_end(ap); 156 if (res > 0) 157 boffs += res; 158 159 if (boffs >= (sizeof(buf) - 1)) 160 boffs = sizeof(buf) - 2; 161 162 buf[boffs] = '\n'; 163 while (boffs && buf[boffs] == '\n') 164 boffs--; 165 boffs++; 166 buf[boffs + 1] = '\0'; 167 168 trace_ext_puts(buf); 169 } 170 171 #else 172 173 /* 174 * In case we have a zero or negative trace level when compiling optee_os, we 175 * have to add stubs to trace functions in case they are used with TA having a 176 * non-zero trace level 177 */ 178 179 void trace_set_level(int level __unused) 180 { 181 } 182 183 int trace_get_level(void) 184 { 185 return 0; 186 } 187 188 void trace_printf(const char *function __unused, int line __unused, 189 int level __unused, bool level_ok __unused, 190 const char *fmt __unused, ...) 191 { 192 } 193 194 #endif 195 196 #if (TRACE_LEVEL >= TRACE_DEBUG) 197 struct strbuf { 198 char buf[MAX_PRINT_SIZE]; 199 char *ptr; 200 }; 201 202 static int __printf(2, 3) append(struct strbuf *sbuf, const char *fmt, ...) 203 { 204 int left; 205 int len; 206 va_list ap; 207 208 if (sbuf->ptr == NULL) 209 sbuf->ptr = sbuf->buf; 210 left = sizeof(sbuf->buf) - (sbuf->ptr - sbuf->buf); 211 va_start(ap, fmt); 212 len = vsnprintk(sbuf->ptr, left, fmt, ap); 213 va_end(ap); 214 if (len < 0) { 215 /* Format error */ 216 return 0; 217 } 218 if (len >= left) { 219 /* Output was truncated */ 220 return 0; 221 } 222 sbuf->ptr += MIN(left, len); 223 return 1; 224 } 225 226 #define PRIxVA_WIDTH ((int)(sizeof(vaddr_t)*2)) 227 228 void dhex_dump(const char *function, int line, int level, 229 const void *buf, int len) 230 { 231 int i; 232 int ok; 233 struct strbuf sbuf; 234 char *in = (char *)buf; 235 236 if (level <= trace_level) { 237 sbuf.ptr = NULL; 238 for (i = 0; i < len; i++) { 239 if ((i % 16) == 0) { 240 ok = append(&sbuf, "%0*" PRIxVA " ", 241 PRIxVA_WIDTH, (vaddr_t)(in + i)); 242 if (!ok) 243 goto err; 244 } 245 ok = append(&sbuf, "%02x ", in[i]); 246 if (!ok) 247 goto err; 248 if ((i % 16) == 7) { 249 ok = append(&sbuf, " "); 250 if (!ok) 251 goto err; 252 } else if ((i % 16) == 15) { 253 trace_printf(function, line, level, true, "%s", 254 sbuf.buf); 255 sbuf.ptr = NULL; 256 } 257 } 258 if (sbuf.ptr) { 259 /* Buffer is not empty: flush it */ 260 trace_printf(function, line, level, true, "%s", 261 sbuf.buf); 262 263 } 264 } 265 return; 266 err: 267 DMSG("Hex dump error"); 268 } 269 #else 270 271 /* 272 * In case we have trace level less than debug when compiling optee_os, we have 273 * to add stubs to trace functions in case they are used with TA having a 274 * a higher trace level 275 */ 276 277 void dhex_dump(const char *function __unused, int line __unused, 278 int level __unused, 279 const void *buf __unused, int len __unused) 280 { 281 } 282 283 #endif 284