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