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