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