1 // SPDX-License-Identifier: BSD-2-Clause 2 /* 3 * Copyright (c) 2014, STMicroelectronics International N.V. 4 */ 5 6 #include <printk.h> 7 #include <stdarg.h> 8 #include <string.h> 9 #include <trace.h> 10 #include <util.h> 11 #include <types_ext.h> 12 13 #if (TRACE_LEVEL > 0) 14 15 #if (TRACE_LEVEL < TRACE_MIN) || (TRACE_LEVEL > TRACE_MAX) 16 #error "Invalid value of TRACE_LEVEL" 17 #endif 18 19 void trace_set_level(int level) 20 { 21 if (((int)level >= TRACE_MIN) && (level <= TRACE_MAX)) 22 trace_level = level; 23 else 24 trace_level = TRACE_MAX; 25 } 26 27 int trace_get_level(void) 28 { 29 return trace_level; 30 } 31 32 static char trace_level_to_string(int level, bool level_ok) 33 { 34 /* 35 * U = Unused 36 * E = Error 37 * I = Information 38 * D = Debug 39 * F = Flow 40 */ 41 static const char lvl_strs[] = { 'U', 'E', 'I', 'D', 'F' }; 42 int l = 0; 43 44 if (!level_ok) 45 return 'M'; 46 47 if ((level >= TRACE_MIN) && (level <= TRACE_MAX)) 48 l = level; 49 50 return lvl_strs[l]; 51 } 52 53 static int print_thread_id(char *buf, size_t bs, int thread_id) 54 { 55 #if CFG_NUM_THREADS > 9 56 int num_thread_digits = 2; 57 #else 58 int num_thread_digits = 1; 59 #endif 60 61 if (thread_id >= 0) 62 return snprintk(buf, bs, "%0*d ", num_thread_digits, thread_id); 63 else 64 return snprintk(buf, bs, "%*s ", num_thread_digits, ""); 65 } 66 67 /* Format trace of user ta. Inline with kernel ta */ 68 void trace_printf(const char *function, int line, int level, bool level_ok, 69 const char *fmt, ...) 70 { 71 va_list ap; 72 char buf[MAX_PRINT_SIZE]; 73 size_t boffs = 0; 74 int res; 75 int thread_id; 76 77 if (level_ok && level > trace_level) 78 return; 79 80 /* Print the type of message */ 81 res = snprintk(buf, sizeof(buf), "%c/", 82 trace_level_to_string(level, level_ok)); 83 if (res < 0) 84 return; 85 boffs += res; 86 87 /* Print the location, i.e., TEE core or TA */ 88 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:", 89 trace_ext_prefix); 90 if (res < 0) 91 return; 92 boffs += res; 93 94 /* Print the Thread ID */ 95 if (level_ok && !(BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 96 thread_id = -1; 97 else 98 thread_id = trace_ext_get_thread_id(); 99 100 res = print_thread_id(buf + boffs, sizeof(buf) - boffs, thread_id); 101 102 if (res < 0) 103 return; 104 boffs += res; 105 106 /* Print the function and line */ 107 if (level_ok && !(BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 108 function = NULL; 109 110 if (function) { 111 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:%d ", 112 function, line); 113 if (res < 0) 114 return; 115 boffs += res; 116 } 117 118 va_start(ap, fmt); 119 res = vsnprintk(buf + boffs, sizeof(buf) - boffs, fmt, ap); 120 va_end(ap); 121 if (res > 0) 122 boffs += res; 123 124 if (boffs >= (sizeof(buf) - 1)) 125 boffs = sizeof(buf) - 2; 126 127 buf[boffs] = '\n'; 128 while (boffs && buf[boffs] == '\n') 129 boffs--; 130 boffs++; 131 buf[boffs + 1] = '\0'; 132 133 trace_ext_puts(buf); 134 } 135 136 #else 137 138 /* 139 * In case we have a zero or negative trace level when compiling optee_os, we 140 * have to add stubs to trace functions in case they are used with TA having a 141 * non-zero trace level 142 */ 143 144 void trace_set_level(int level __unused) 145 { 146 } 147 148 int trace_get_level(void) 149 { 150 return 0; 151 } 152 153 void trace_printf(const char *function __unused, int line __unused, 154 int level __unused, bool level_ok __unused, 155 const char *fmt __unused, ...) 156 { 157 } 158 159 #endif 160 161 #if (TRACE_LEVEL >= TRACE_DEBUG) 162 struct strbuf { 163 char buf[MAX_PRINT_SIZE]; 164 char *ptr; 165 }; 166 167 static int __printf(2, 3) append(struct strbuf *sbuf, const char *fmt, ...) 168 { 169 int left; 170 int len; 171 va_list ap; 172 173 if (sbuf->ptr == NULL) 174 sbuf->ptr = sbuf->buf; 175 left = sizeof(sbuf->buf) - (sbuf->ptr - sbuf->buf); 176 va_start(ap, fmt); 177 len = vsnprintk(sbuf->ptr, left, fmt, ap); 178 va_end(ap); 179 if (len < 0) { 180 /* Format error */ 181 return 0; 182 } 183 if (len >= left) { 184 /* Output was truncated */ 185 return 0; 186 } 187 sbuf->ptr += MIN(left, len); 188 return 1; 189 } 190 191 #define PRIxVA_WIDTH ((int)(sizeof(vaddr_t)*2)) 192 193 void dhex_dump(const char *function, int line, int level, 194 const void *buf, int len) 195 { 196 int i; 197 int ok; 198 struct strbuf sbuf; 199 char *in = (char *)buf; 200 201 if (level <= trace_level) { 202 sbuf.ptr = NULL; 203 for (i = 0; i < len; i++) { 204 if ((i % 16) == 0) { 205 ok = append(&sbuf, "%0*" PRIxVA " ", 206 PRIxVA_WIDTH, (vaddr_t)(in + i)); 207 if (!ok) 208 goto err; 209 } 210 ok = append(&sbuf, "%02x ", in[i]); 211 if (!ok) 212 goto err; 213 if ((i % 16) == 7) { 214 ok = append(&sbuf, " "); 215 if (!ok) 216 goto err; 217 } else if ((i % 16) == 15) { 218 trace_printf(function, line, level, true, "%s", 219 sbuf.buf); 220 sbuf.ptr = NULL; 221 } 222 } 223 if (sbuf.ptr) { 224 /* Buffer is not empty: flush it */ 225 trace_printf(function, line, level, true, "%s", 226 sbuf.buf); 227 228 } 229 } 230 return; 231 err: 232 DMSG("Hex dump error"); 233 } 234 #else 235 236 /* 237 * In case we have trace level less than debug when compiling optee_os, we have 238 * to add stubs to trace functions in case they are used with TA having a 239 * a higher trace level 240 */ 241 242 void dhex_dump(const char *function __unused, int line __unused, 243 int level __unused, 244 const void *buf __unused, int len __unused) 245 { 246 } 247 248 #endif 249