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