1 /* 2 * Copyright (c) 2014, STMicroelectronics International N.V. 3 * All rights reserved. 4 * 5 * Redistribution and use in source and binary forms, with or without 6 * modification, are permitted provided that the following conditions are met: 7 * 8 * 1. Redistributions of source code must retain the above copyright notice, 9 * this list of conditions and the following disclaimer. 10 * 11 * 2. Redistributions in binary form must reproduce the above copyright notice, 12 * this list of conditions and the following disclaimer in the documentation 13 * and/or other materials provided with the distribution. 14 * 15 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 16 * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 17 * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 18 * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE 19 * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 20 * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 21 * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 22 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 23 * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 24 * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 25 * POSSIBILITY OF SUCH DAMAGE. 26 */ 27 28 #include <printk.h> 29 #include <stdarg.h> 30 #include <string.h> 31 #include <trace.h> 32 #include <util.h> 33 #include <types_ext.h> 34 35 #if (TRACE_LEVEL > 0) 36 37 #if (TRACE_LEVEL < TRACE_MIN) || (TRACE_LEVEL > TRACE_MAX) 38 #error "Invalid value of TRACE_LEVEL" 39 #endif 40 41 void trace_set_level(int level) 42 { 43 if (((int)level >= TRACE_MIN) && (level <= TRACE_MAX)) 44 trace_level = level; 45 else 46 trace_level = TRACE_MAX; 47 } 48 49 int trace_get_level(void) 50 { 51 return trace_level; 52 } 53 54 static char trace_level_to_string(int level, bool level_ok) 55 { 56 /* 57 * U = Unused 58 * E = Error 59 * I = Information 60 * D = Debug 61 * F = Flow 62 */ 63 static const char lvl_strs[] = { 'U', 'E', 'I', 'D', 'F' }; 64 int l = 0; 65 66 if (!level_ok) 67 return 'M'; 68 69 if ((level >= TRACE_MIN) && (level <= TRACE_MAX)) 70 l = level; 71 72 return lvl_strs[l]; 73 } 74 75 static int print_thread_id(char *buf, size_t bs, int thread_id) 76 { 77 #if CFG_NUM_THREADS > 9 78 int num_thread_digits = 2; 79 #else 80 int num_thread_digits = 1; 81 #endif 82 83 if (thread_id >= 0) 84 return snprintk(buf, bs, "%0*d ", num_thread_digits, thread_id); 85 else 86 return snprintk(buf, bs, "%*s ", num_thread_digits, ""); 87 } 88 89 /* Format trace of user ta. Inline with kernel ta */ 90 void trace_printf(const char *function, int line, int level, bool level_ok, 91 const char *fmt, ...) 92 { 93 va_list ap; 94 char buf[MAX_PRINT_SIZE]; 95 size_t boffs = 0; 96 int res; 97 int thread_id; 98 99 if (level_ok && level > trace_level) 100 return; 101 102 /* Print the type of message */ 103 res = snprintk(buf, sizeof(buf), "%c/", 104 trace_level_to_string(level, level_ok)); 105 if (res < 0) 106 return; 107 boffs += res; 108 109 /* Print the location, i.e., TEE core or TA */ 110 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:", 111 trace_ext_prefix); 112 if (res < 0) 113 return; 114 boffs += res; 115 116 /* Print the Thread ID */ 117 if (level_ok && !(BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 118 thread_id = -1; 119 else 120 thread_id = trace_ext_get_thread_id(); 121 122 res = print_thread_id(buf + boffs, sizeof(buf) - boffs, thread_id); 123 124 if (res < 0) 125 return; 126 boffs += res; 127 128 /* Print the function and line */ 129 if (level_ok && !(BIT(level) & CFG_MSG_LONG_PREFIX_MASK)) 130 function = NULL; 131 132 if (function) { 133 res = snprintk(buf + boffs, sizeof(buf) - boffs, "%s:%d ", 134 function, line); 135 if (res < 0) 136 return; 137 boffs += res; 138 } 139 140 va_start(ap, fmt); 141 res = vsnprintk(buf + boffs, sizeof(buf) - boffs, fmt, ap); 142 va_end(ap); 143 if (res > 0) 144 boffs += res; 145 146 if (boffs >= (sizeof(buf) - 1)) 147 boffs = sizeof(buf) - 2; 148 149 buf[boffs] = '\n'; 150 while (boffs && buf[boffs] == '\n') 151 boffs--; 152 boffs++; 153 buf[boffs + 1] = '\0'; 154 155 trace_ext_puts(buf); 156 } 157 158 #else 159 160 /* 161 * In case we have a zero or negative trace level when compiling optee_os, we 162 * have to add stubs to trace functions in case they are used with TA having a 163 * non-zero trace level 164 */ 165 166 void trace_set_level(int level __unused) 167 { 168 } 169 170 int trace_get_level(void) 171 { 172 return 0; 173 } 174 175 void trace_printf(const char *function __unused, int line __unused, 176 int level __unused, bool level_ok __unused, 177 const char *fmt __unused, ...) 178 { 179 } 180 181 #endif 182 183 #if (TRACE_LEVEL >= TRACE_DEBUG) 184 struct strbuf { 185 char buf[MAX_PRINT_SIZE]; 186 char *ptr; 187 }; 188 189 static int __printf(2, 3) append(struct strbuf *sbuf, const char *fmt, ...) 190 { 191 int left; 192 int len; 193 va_list ap; 194 195 if (sbuf->ptr == NULL) 196 sbuf->ptr = sbuf->buf; 197 left = sizeof(sbuf->buf) - (sbuf->ptr - sbuf->buf); 198 va_start(ap, fmt); 199 len = vsnprintk(sbuf->ptr, left, fmt, ap); 200 va_end(ap); 201 if (len < 0) { 202 /* Format error */ 203 return 0; 204 } 205 if (len >= left) { 206 /* Output was truncated */ 207 return 0; 208 } 209 sbuf->ptr += MIN(left, len); 210 return 1; 211 } 212 213 #define PRIxVA_WIDTH ((int)(sizeof(vaddr_t)*2)) 214 215 void dhex_dump(const char *function, int line, int level, 216 const void *buf, int len) 217 { 218 int i; 219 int ok; 220 struct strbuf sbuf; 221 char *in = (char *)buf; 222 223 if (level <= trace_level) { 224 sbuf.ptr = NULL; 225 for (i = 0; i < len; i++) { 226 if ((i % 16) == 0) { 227 ok = append(&sbuf, "%0*" PRIxVA " ", 228 PRIxVA_WIDTH, (vaddr_t)(in + i)); 229 if (!ok) 230 goto err; 231 } 232 ok = append(&sbuf, "%02x ", in[i]); 233 if (!ok) 234 goto err; 235 if ((i % 16) == 7) { 236 ok = append(&sbuf, " "); 237 if (!ok) 238 goto err; 239 } else if ((i % 16) == 15) { 240 trace_printf(function, line, level, true, "%s", 241 sbuf.buf); 242 sbuf.ptr = NULL; 243 } 244 } 245 if (sbuf.ptr) { 246 /* Buffer is not empty: flush it */ 247 trace_printf(function, line, level, true, "%s", 248 sbuf.buf); 249 250 } 251 } 252 return; 253 err: 254 DMSG("Hex dump error"); 255 } 256 #else 257 258 /* 259 * In case we have trace level less than debug when compiling optee_os, we have 260 * to add stubs to trace functions in case they are used with TA having a 261 * a higher trace level 262 */ 263 264 void dhex_dump(const char *function __unused, int line __unused, 265 int level __unused, 266 const void *buf __unused, int len __unused) 267 { 268 } 269 270 #endif 271