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) - 1)) 105 boffs = sizeof(buf) - 2; 106 107 buf[boffs] = '\n'; 108 buf[boffs + 1] = '\0'; 109 110 trace_ext_puts(buf); 111 } 112 113 #else 114 115 /* 116 * In case we have a zero or negative trace level when compiling optee_os, we 117 * have to add stubs to trace functions in case they are used with TA having a 118 * non-zero trace level 119 */ 120 121 void trace_set_level(int level __unused) 122 { 123 } 124 125 int trace_get_level(void) 126 { 127 return 0; 128 } 129 130 void trace_printf(const char *function __unused, int line __unused, 131 int level __unused, bool level_ok __unused, 132 const char *fmt __unused, ...) 133 { 134 } 135 136 #endif 137 138 #if (TRACE_LEVEL >= TRACE_DEBUG) 139 struct strbuf { 140 char buf[MAX_PRINT_SIZE]; 141 char *ptr; 142 }; 143 144 static int __printf(2, 3) append(struct strbuf *sbuf, const char *fmt, ...) 145 { 146 int left; 147 int len; 148 va_list ap; 149 150 if (sbuf->ptr == NULL) 151 sbuf->ptr = sbuf->buf; 152 left = sizeof(sbuf->buf) - (sbuf->ptr - sbuf->buf); 153 va_start(ap, fmt); 154 len = vsnprintk(sbuf->ptr, left, fmt, ap); 155 va_end(ap); 156 if (len < 0) { 157 /* Format error */ 158 return 0; 159 } 160 if (len >= left) { 161 /* Output was truncated */ 162 return 0; 163 } 164 sbuf->ptr += MIN(left, len); 165 return 1; 166 } 167 168 #define PRIxVA_WIDTH ((int)(sizeof(vaddr_t)*2)) 169 170 void dhex_dump(const char *function, int line, int level, 171 const void *buf, int len) 172 { 173 int i; 174 int ok; 175 struct strbuf sbuf; 176 char *in = (char *)buf; 177 178 if (level <= trace_level) { 179 sbuf.ptr = NULL; 180 for (i = 0; i < len; i++) { 181 if ((i % 16) == 0) { 182 ok = append(&sbuf, "%0*" PRIxVA " ", 183 PRIxVA_WIDTH, (vaddr_t)(in + i)); 184 if (!ok) 185 goto err; 186 } 187 ok = append(&sbuf, "%02x ", in[i]); 188 if (!ok) 189 goto err; 190 if ((i % 16) == 7) { 191 ok = append(&sbuf, " "); 192 if (!ok) 193 goto err; 194 } else if ((i % 16) == 15) { 195 trace_printf(function, line, level, true, "%s", 196 sbuf.buf); 197 sbuf.ptr = NULL; 198 } 199 } 200 if (sbuf.ptr) { 201 /* Buffer is not empty: flush it */ 202 trace_printf(function, line, level, true, "%s", 203 sbuf.buf); 204 205 } 206 } 207 return; 208 err: 209 DMSG("Hex dump error"); 210 } 211 #else 212 213 /* 214 * In case we have trace level less than debug when compiling optee_os, we have 215 * to add stubs to trace functions in case they are used with TA having a 216 * a higher trace level 217 */ 218 219 void dhex_dump(const char *function __unused, int line __unused, 220 int level __unused, 221 const void *buf __unused, int len __unused) 222 { 223 } 224 225 #endif 226