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