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 const char *trace_level_to_string(int level, bool level_ok) 55 { 56 static const char lvl_strs[][4] = { 57 "UKN", "ERR", "INF", "DBG", "FLW" }; 58 int l = 0; 59 60 if (!level_ok) 61 return "MSG"; 62 63 if ((level >= TRACE_MIN) && (level <= TRACE_MAX)) 64 l = level; 65 66 return lvl_strs[l]; 67 } 68 69 /* Format trace of user ta. Inline with kernel ta */ 70 void trace_printf(const char *function, int line, int level, bool level_ok, 71 const char *fmt, ...) 72 { 73 va_list ap; 74 char buf[MAX_PRINT_SIZE]; 75 size_t boffs = 0; 76 int res; 77 78 if (level_ok && level > trace_level) 79 return; 80 81 if (function) { 82 int thread_id = trace_ext_get_thread_id(); 83 84 if (thread_id >= 0) 85 res = snprintk(buf, sizeof(buf), "%s [0x%x] %s:%s:%d: ", 86 trace_level_to_string(level, level_ok), 87 thread_id, trace_ext_prefix, 88 function, line); 89 else 90 res = snprintk(buf, sizeof(buf), "%s %s:%s:%d: ", 91 trace_level_to_string(level, level_ok), 92 trace_ext_prefix, function, line); 93 if (res < 0) 94 return; /* "Can't happen" */ 95 boffs = res; 96 } 97 98 va_start(ap, fmt); 99 res = vsnprintk(buf + boffs, sizeof(buf) - boffs, fmt, ap); 100 va_end(ap); 101 if (res > 0) 102 boffs += res; 103 104 if (boffs >= sizeof(buf)) { 105 boffs = sizeof(buf) - 2; 106 /* Make there's a newline at the end */ 107 buf[boffs] = '\n'; 108 } else if (buf[boffs - 1] != '\n') { 109 /* Append a newline */ 110 buf[boffs] = '\n'; 111 buf[boffs + 1] = '\0'; 112 } 113 114 trace_ext_puts(buf); 115 } 116 117 #else 118 119 /* 120 * In case we have a zero or negative trace level when compiling optee_os, we 121 * have to add stubs to trace functions in case they are used with TA having a 122 * non-zero trace level 123 */ 124 125 void trace_set_level(int level __unused) 126 { 127 } 128 129 int trace_get_level(void) 130 { 131 return 0; 132 } 133 134 void trace_printf(const char *function __unused, int line __unused, 135 int level __unused, bool level_ok __unused, 136 const char *fmt __unused, ...) 137 { 138 } 139 140 #endif 141 142 #if (TRACE_LEVEL >= TRACE_DEBUG) 143 struct strbuf { 144 char buf[MAX_PRINT_SIZE]; 145 char *ptr; 146 }; 147 148 static int __printf(2, 3) append(struct strbuf *sbuf, const char *fmt, ...) 149 { 150 int left; 151 int len; 152 va_list ap; 153 154 if (sbuf->ptr == NULL) 155 sbuf->ptr = sbuf->buf; 156 left = sizeof(sbuf->buf) - (sbuf->ptr - sbuf->buf); 157 va_start(ap, fmt); 158 len = vsnprintk(sbuf->ptr, left, fmt, ap); 159 va_end(ap); 160 if (len < 0) { 161 /* Format error */ 162 return 0; 163 } 164 if (len >= left) { 165 /* Output was truncated */ 166 return 0; 167 } 168 sbuf->ptr += MIN(left, len); 169 return 1; 170 } 171 172 #define PRIxVA_WIDTH ((int)(sizeof(vaddr_t)*2)) 173 174 void dhex_dump(const char *function, int line, int level, 175 const void *buf, int len) 176 { 177 int i; 178 int ok; 179 struct strbuf sbuf; 180 char *in = (char *)buf; 181 182 if (level <= trace_level) { 183 sbuf.ptr = NULL; 184 for (i = 0; i < len; i++) { 185 if ((i % 16) == 0) { 186 ok = append(&sbuf, "%0*" PRIxVA " ", 187 PRIxVA_WIDTH, (vaddr_t)(in + i)); 188 if (!ok) 189 goto err; 190 } 191 ok = append(&sbuf, "%02x ", in[i]); 192 if (!ok) 193 goto err; 194 if ((i % 16) == 7) { 195 ok = append(&sbuf, " "); 196 if (!ok) 197 goto err; 198 } else if ((i % 16) == 15) { 199 trace_printf(function, line, level, true, "%s", 200 sbuf.buf); 201 sbuf.ptr = NULL; 202 } 203 } 204 if (sbuf.ptr) { 205 /* Buffer is not empty: flush it */ 206 trace_printf(function, line, level, true, "%s", 207 sbuf.buf); 208 209 } 210 } 211 return; 212 err: 213 DMSG("Hex dump error"); 214 } 215 #else 216 217 /* 218 * In case we have trace level less than debug when compiling optee_os, we have 219 * to add stubs to trace functions in case they are used with TA having a 220 * a higher trace level 221 */ 222 223 void dhex_dump(const char *function __unused, int line __unused, 224 int level __unused, 225 const void *buf __unused, int len __unused) 226 { 227 } 228 229 #endif 230