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