ns_trace.c

00001 /*
00002  * Copyright (c) 2014-2015 ARM Limited. All rights reserved.
00003  * SPDX-License-Identifier: Apache-2.0
00004  * Licensed under the Apache License, Version 2.0 (the License); you may
00005  * not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  * http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an AS IS BASIS, WITHOUT
00012  * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 #include <stdio.h>
00017 #include <string.h>
00018 #include <stdarg.h>
00019 #include <stdlib.h>
00020 
00021 #define HAVE_DEBUG 1
00022 
00023 #include "ns_trace.h"
00024 #include "ip6string.h"
00025 #include "common_functions.h"
00026 
00027 #if defined(_WIN32) || defined(__unix__) || defined(__unix) || defined(unix) || (defined(YOTTA_CFG) && !defined(NDEBUG))
00028 //NOTE! It is not allowed to use this MEM_ALLOC/MEM_FREE from interrupt context.
00029 #ifndef MEM_ALLOC
00030 #define MEM_ALLOC malloc
00031 #endif
00032 #ifndef MEM_FREE
00033 #define MEM_FREE free
00034 #endif
00035 #else
00036 #include "nsdynmemLIB.h"
00037 #ifndef MEM_ALLOC
00038 #define MEM_ALLOC ns_dyn_mem_alloc
00039 #endif
00040 #ifndef MEM_FREE
00041 #define MEM_FREE ns_dyn_mem_free
00042 #endif
00043 #endif
00044 
00045 #define VT100_COLOR_ERROR "\x1b[31m"
00046 #define VT100_COLOR_WARN  "\x1b[33m"
00047 #define VT100_COLOR_INFO  "\x1b[39m"
00048 #define VT100_COLOR_DEBUG "\x1b[90m"
00049 
00050 /** default max trace line size in bytes */
00051 #define DEFAULT_TRACE_LINE_LENGTH     1024
00052 /** default max temporary buffer size in bytes, used in
00053     trace_ipv6, trace_array and trace_strn */
00054 #define DEFAULT_TRACE_TMP_LINE_LEN        128
00055 /** default max filters (include/exclude) length in bytes */
00056 #define DEFAULT_TRACE_FILTER_LENGTH       24
00057 
00058 /** default print function, just redirect str to printf */
00059 static void default_print(const char *str);
00060 
00061 typedef struct {
00062     /** trace configuration bits */
00063     uint8_t trace_config;
00064     /** exclude filters list, related group name */
00065     char *filters_exclude;
00066     /** include filters list, related group name */
00067     char *filters_include;
00068     /** Filters length */
00069     int filters_length;
00070     /** trace line */
00071     char *line;
00072     /** trace line length */
00073     int line_length;
00074     /** temporary data */
00075     char *tmp_data;
00076     /** temporary data array length */
00077     int tmp_data_length;
00078     /** temporary data pointer */
00079     char *tmp_data_ptr;
00080 
00081     /** prefix function, which can be used to put time to the trace line */
00082     char *(*prefix_f)(size_t);
00083     /** suffix function, which can be used to some string to the end of trace line */
00084     char *(*suffix_f)(void);
00085     /** print out function. Can be redirect to flash for example. */
00086     void (*printf)(const char *);
00087     /** print out function for TRACE_LEVEL_CMD */
00088     void (*cmd_printf)(const char *);
00089 } trace_s;
00090 
00091 static trace_s m_trace = {
00092     .filters_exclude = 0,
00093     .filters_include = 0,
00094     .line = 0,
00095     .tmp_data = 0,
00096     .prefix_f = 0,
00097     .suffix_f = 0,
00098     .printf  = 0,
00099     .cmd_printf = 0
00100 };
00101 
00102 int trace_init(void)
00103 {
00104     m_trace.trace_config = TRACE_MODE_COLOR | TRACE_ACTIVE_LEVEL_ALL | TRACE_CARRIAGE_RETURN;
00105     m_trace.line_length = DEFAULT_TRACE_LINE_LENGTH;
00106     if (m_trace.line == NULL) {
00107         m_trace.line = MEM_ALLOC(m_trace.line_length);
00108     }
00109     m_trace.tmp_data_length = DEFAULT_TRACE_TMP_LINE_LEN;
00110     if (m_trace.tmp_data == NULL) {
00111         m_trace.tmp_data = MEM_ALLOC(m_trace.tmp_data_length);
00112     }
00113     m_trace.tmp_data_ptr = m_trace.tmp_data;
00114     m_trace.filters_length = DEFAULT_TRACE_FILTER_LENGTH;
00115     if (m_trace.filters_exclude == NULL) {
00116         m_trace.filters_exclude = MEM_ALLOC(m_trace.filters_length);
00117     }
00118     if (m_trace.filters_include == NULL) {
00119         m_trace.filters_include = MEM_ALLOC(m_trace.filters_length);
00120     }
00121 
00122     if (m_trace.line == NULL ||
00123             m_trace.tmp_data == NULL ||
00124             m_trace.filters_exclude == NULL  ||
00125             m_trace.filters_include == NULL) {
00126         //memory allocation fail
00127         trace_free();
00128         return -1;
00129     }
00130     memset(m_trace.tmp_data, 0, m_trace.tmp_data_length);
00131     memset(m_trace.filters_exclude, 0, m_trace.filters_length);
00132     memset(m_trace.filters_include, 0, m_trace.filters_length);
00133     memset(m_trace.line, 0, m_trace.line_length);
00134 
00135     m_trace.prefix_f = 0;
00136     m_trace.suffix_f = 0;
00137     m_trace.printf = default_print;
00138     m_trace.cmd_printf = 0;
00139 
00140     return 0;
00141 }
00142 void trace_free(void)
00143 {
00144     MEM_FREE(m_trace.line);
00145     m_trace.line_length = 0;
00146     m_trace.line = 0;
00147     MEM_FREE(m_trace.tmp_data);
00148     m_trace.tmp_data = 0;
00149     m_trace.tmp_data_ptr = 0;
00150     MEM_FREE(m_trace.filters_exclude);
00151     m_trace.filters_exclude = 0;
00152     MEM_FREE(m_trace.filters_include);
00153     m_trace.filters_include = 0;
00154     m_trace.filters_length = 0;
00155 }
00156 /** @TODO do we need dynamically change trace buffer sizes ?
00157 // reconfigure trace buffer sizes
00158 void set_trace_buffer_sizes(int lineLength, int tmpLength)
00159 {
00160   REALLOC( m_trace.line, dataLength );
00161   REALLOC( m_trace.tmp_data, tmpLength);
00162   m_trace.tmp_data_length = tmpLength;
00163 }
00164 */
00165 void set_trace_config(uint8_t config)
00166 {
00167     m_trace.trace_config = config;
00168 }
00169 uint8_t get_trace_config(void)
00170 {
00171     return m_trace.trace_config;
00172 }
00173 void set_trace_prefix_function(char *(*pref_f)(size_t))
00174 {
00175     m_trace.prefix_f = pref_f;
00176 }
00177 void set_trace_suffix_function(char *(*suffix_f)(void))
00178 {
00179     m_trace.suffix_f = suffix_f;
00180 }
00181 void set_trace_print_function(void (*printf)(const char *))
00182 {
00183     m_trace.printf = printf;
00184 }
00185 void set_trace_cmdprint_function(void (*printf)(const char *))
00186 {
00187     m_trace.cmd_printf = printf;
00188 }
00189 void set_trace_exclude_filters(char *filters)
00190 {
00191     if (filters) {
00192         (void)strncpy(m_trace.filters_exclude, filters, m_trace.filters_length);
00193     } else {
00194         m_trace.filters_exclude[0] = 0;
00195     }
00196 }
00197 const char *get_trace_exclude_filters(void)
00198 {
00199     return m_trace.filters_exclude;
00200 }
00201 const char *get_trace_include_filters(void)
00202 {
00203     return m_trace.filters_include;
00204 }
00205 void set_trace_include_filters(char *filters)
00206 {
00207     if (filters) {
00208         (void)strncpy(m_trace.filters_include, filters, m_trace.filters_length);
00209     } else {
00210         m_trace.filters_include[0] = 0;
00211     }
00212 }
00213 static int8_t trace_skip(int8_t dlevel, const char *grp)
00214 {
00215     if (dlevel >= 0 && grp != 0) {
00216         // filter debug prints only when dlevel is >0 and grp is given
00217 
00218         /// @TODO this could be much better..
00219         if (m_trace.filters_exclude[0] != '\0' &&
00220                 strstr(m_trace.filters_exclude, grp) != 0) {
00221             //grp was in exclude list
00222             return 1;
00223         }
00224         if (m_trace.filters_include[0] != '\0' &&
00225                 strstr(m_trace.filters_include, grp) == 0) {
00226             //grp was in include list
00227             return 1;
00228         }
00229     }
00230     return 0;
00231 }
00232 static void default_print(const char *str)
00233 {
00234     puts(str);
00235 }
00236 void tracef(uint8_t dlevel, const char *grp, const char *fmt, ...)
00237 {
00238     if (m_trace.line == NULL) {
00239         // Quite likely the trace_init() has not been called yet,
00240         // but it is better to just shut up instead of crashing with
00241         // null pointer dereference. 
00242         return;
00243     }
00244     
00245     m_trace.line[0] = 0; //by default trace is empty
00246     if (trace_skip(dlevel, grp) || fmt == 0 || grp == 0) {
00247         return;
00248     }
00249     if ((m_trace.trace_config & TRACE_MASK_LEVEL) &  dlevel) {
00250         bool color = (m_trace.trace_config & TRACE_MODE_COLOR) != 0;
00251         bool plain = (m_trace.trace_config & TRACE_MODE_PLAIN) != 0;
00252         bool cr    = (m_trace.trace_config & TRACE_CARRIAGE_RETURN) != 0;
00253         //printf("CFG: 0x%02x, plain: %i, color: %i, cr: %i\n", m_trace.trace_config, plain, color, cr);
00254 
00255         int retval = 0, bLeft = m_trace.line_length;
00256         char *ptr = m_trace.line;
00257         if (plain == true || dlevel == TRACE_LEVEL_CMD) {
00258             va_list ap;
00259             va_start(ap, fmt);
00260             //add trace data
00261             retval = vsnprintf(ptr, bLeft, fmt, ap);
00262             va_end(ap);
00263             if (dlevel == TRACE_LEVEL_CMD && m_trace.cmd_printf) {
00264                 m_trace.cmd_printf(m_trace.line);
00265                 m_trace.cmd_printf("\n");
00266             } else {
00267                 //print out whole data
00268                 m_trace.printf(m_trace.line);
00269             }
00270         } else {
00271             if (color) {
00272                 if (cr) {
00273                     retval = snprintf(ptr, bLeft, "\r\x1b[2K");
00274                     if (retval >= bLeft) {
00275                         retval = 0;
00276                     }
00277                     if (retval > 0) {
00278                         ptr += retval;
00279                         bLeft -= retval;
00280                     }
00281                 }
00282                 if (bLeft > 0) {
00283                     //include color in ANSI/VT100 escape code
00284                     switch (dlevel) {
00285                         case (TRACE_LEVEL_ERROR):
00286                             retval = snprintf(ptr, bLeft, "%s", VT100_COLOR_ERROR);
00287                             break;
00288                         case (TRACE_LEVEL_WARN):
00289                             retval = snprintf(ptr, bLeft, "%s", VT100_COLOR_WARN);
00290                             break;
00291                         case (TRACE_LEVEL_INFO):
00292                             retval = snprintf(ptr, bLeft, "%s", VT100_COLOR_INFO);
00293                             break;
00294                         case (TRACE_LEVEL_DEBUG):
00295                             retval = snprintf(ptr, bLeft, "%s", VT100_COLOR_DEBUG);
00296                             break;
00297                         default:
00298                             color = 0; //avoid unneeded color-terminate code
00299                             retval = 0;
00300                             break;
00301                     }
00302                     if (retval >= bLeft) {
00303                         retval = 0;
00304                     }
00305                     if (retval > 0 && color) {
00306                         ptr += retval;
00307                         bLeft -= retval;
00308                     }
00309                 }
00310 
00311             }
00312             if (bLeft > 0 && m_trace.prefix_f) {
00313                 va_list ap;
00314                 va_start(ap, fmt);
00315                 //find out length of body
00316                 size_t sz = 0;
00317                 sz = vsnprintf(NULL, 0, fmt, ap) + retval + (retval ? 4 : 0);
00318                 //add prefix string
00319                 retval = snprintf(ptr, bLeft, "%s", m_trace.prefix_f(sz));
00320                 if (retval >= bLeft) {
00321                     retval = 0;
00322                 }
00323                 if (retval > 0) {
00324                     ptr += retval;
00325                     bLeft -= retval;
00326                 }
00327                 va_end(ap);
00328             }
00329             if (bLeft > 0) {
00330                 //add group tag
00331                 switch (dlevel) {
00332                     case (TRACE_LEVEL_ERROR):
00333                         retval = snprintf(ptr, bLeft, "[ERR ][%-4s]: ", grp);
00334                         break;
00335                     case (TRACE_LEVEL_WARN):
00336                         retval = snprintf(ptr, bLeft, "[WARN][%-4s]: ", grp);
00337                         break;
00338                     case (TRACE_LEVEL_INFO):
00339                         retval = snprintf(ptr, bLeft, "[INFO][%-4s]: ", grp);
00340                         break;
00341                     case (TRACE_LEVEL_DEBUG):
00342                         retval = snprintf(ptr, bLeft, "[DBG ][%-4s]: ", grp);
00343                         break;
00344                     default:
00345                         retval = snprintf(ptr, bLeft, "              ");
00346                         break;
00347                 }
00348                 if (retval >= bLeft) {
00349                     retval = 0;
00350                 }
00351                 if (retval > 0) {
00352                     ptr += retval;
00353                     bLeft -= retval;
00354                 }
00355             }
00356             if (retval > 0 && bLeft > 0) {
00357                 va_list ap;
00358                 va_start(ap, fmt);
00359                 //add trace text
00360                 retval = vsnprintf(ptr, bLeft, fmt, ap);
00361                 if (retval >= bLeft) {
00362                     retval = 0;
00363                 }
00364                 if (retval > 0) {
00365                     ptr += retval;
00366                     bLeft -= retval;
00367                 }
00368                 va_end(ap);
00369             }
00370 
00371             if (retval > 0 && bLeft > 0  && m_trace.suffix_f) {
00372                 //add suffix string
00373                 retval = snprintf(ptr, bLeft, "%s", m_trace.suffix_f());
00374                 if (retval >= bLeft) {
00375                     retval = 0;
00376                 }
00377                 if (retval > 0) {
00378                     ptr += retval;
00379                     bLeft -= retval;
00380                 }
00381             }
00382 
00383             if (retval > 0 && bLeft > 0  && color) {
00384                 //add zero color VT100 when color mode
00385                 retval = snprintf(ptr, bLeft, "\x1b[0m");
00386                 if (retval >= bLeft) {
00387                     retval = 0;
00388                 }
00389                 if (retval > 0) {
00390                     ptr += retval;
00391                     bLeft -= retval;
00392                 }
00393             }
00394             //print out whole data
00395             m_trace.printf(m_trace.line);
00396         }
00397         //return tmp data pointer back to the beginning
00398         m_trace.tmp_data_ptr = m_trace.tmp_data;
00399     }
00400 }
00401 const char *trace_last(void)
00402 {
00403     return m_trace.line;
00404 }
00405 /* Helping functions */
00406 #define tmp_data_left()  m_trace.tmp_data_length-(m_trace.tmp_data_ptr-m_trace.tmp_data)
00407 char *trace_ipv6(const void *addr_ptr)
00408 {
00409     char *str = m_trace.tmp_data_ptr;
00410     if (str == NULL) {
00411         return "";
00412     }
00413     if (tmp_data_left() < 41) {
00414         return "";
00415     }
00416     if (addr_ptr == NULL) {
00417         return "<null>";
00418     }
00419     str[0] = 0;
00420     ip6tos(addr_ptr, str);
00421     m_trace.tmp_data_ptr += strlen(str) + 1;
00422     return str;
00423 }
00424 char *trace_ipv6_prefix(const uint8_t *prefix, uint8_t prefix_len)
00425 {
00426     char *str = m_trace.tmp_data_ptr;
00427     int retval, bLeft = tmp_data_left();
00428     char tmp[40];
00429     uint8_t addr[16] = {0};
00430 
00431     if (str == NULL) {
00432         return "";
00433     }
00434     if (bLeft < 43) {
00435         return "";
00436     }
00437 
00438     if (prefix_len != 0) {
00439         if (prefix == NULL || prefix_len > 128) {
00440             return "<err>";
00441         }
00442         bitcopy(addr, prefix, prefix_len);
00443     }
00444 
00445     ip6tos(addr, tmp);
00446     retval = snprintf(str, bLeft, "%s/%u", tmp, prefix_len);
00447     if (retval <= 0 || retval > bLeft) {
00448         return "";
00449     }
00450 
00451     m_trace.tmp_data_ptr += retval + 1;
00452     return str;
00453 }
00454 char *trace_array(const uint8_t *buf, uint16_t len)
00455 {
00456     int i, retval, bLeft = tmp_data_left();
00457     char *str, *wptr;
00458     str = m_trace.tmp_data_ptr;
00459     if (str == NULL) {
00460         return "";
00461     }
00462     if (buf == NULL) {
00463         return "<null>";
00464     }
00465     wptr = str;
00466     wptr[0] = 0;
00467     const uint8_t *ptr = buf;
00468     for (i = 0; i < len; i++) {
00469         if (bLeft <= 0) {
00470             break;
00471         }
00472         retval = snprintf(wptr, bLeft, "%02x:", *ptr++);
00473         if (retval <= 0 || retval > bLeft) {
00474             break;
00475         }
00476         bLeft -= retval;
00477         wptr += retval;
00478     }
00479     if (wptr > str) {
00480         *(wptr - 1) = 0;    //null to replace last ':' character
00481     }
00482     m_trace.tmp_data_ptr = wptr;
00483     return str;
00484 }
00485 
00486 // rest of debug print functions will be obsolete and will be overridden with new trace interface..
00487 void debugf(const char *fmt, ...)
00488 {
00489     va_list ap;
00490     va_start(ap, fmt);
00491     vprintf(fmt, ap);
00492     va_end(ap);
00493 }
00494 
00495 void debug (const char *s)
00496 {
00497     fputs(s, stdout);
00498 }
00499 
00500 void debug_put(char c)
00501 {
00502     putchar(c);
00503 }
00504 
00505 void debug_hex(uint8_t x)
00506 {
00507     printf("%02x", x);
00508 }
00509 void debug_int(int i)
00510 {
00511     printf("%d", i);
00512 }
00513 void printf_array(const void *buf , uint16_t len)
00514 {
00515     int i;
00516     const uint8_t *ptr = buf;
00517     for (i = 0; i < len; i++) {
00518         if (i && (0 == i % 16)) {
00519             putchar('\n');
00520         }
00521         printf("%02x:", *ptr++);
00522 
00523     }
00524     putchar('\n');
00525 }
00526 void printf_ipv6_address(const void *addr_ptr)
00527 {
00528     char buf[40] = {0};
00529     ip6tos(addr_ptr, buf);
00530     printf("%s\n", buf);
00531 }
00532 void printf_string(const void *ptr, uint16_t len)
00533 {
00534     printf("%.*s\n", len, (const char *)ptr);
00535 }