1 /*********************************************************************************************************
2 * Software License Agreement (BSD License) *
3 * Author: Sebastien Decugis <sdecugis@freediameter.net> *
4 * *
5 * Copyright (c) 2020, WIDE Project and NICT *
6 * All rights reserved. *
7 * *
8 * Redistribution and use of this software in source and binary forms, with or without modification, are *
9 * permitted provided that the following conditions are met: *
10 * *
11 * * Redistributions of source code must retain the above *
12 * copyright notice, this list of conditions and the *
13 * following disclaimer. *
14 * *
15 * * Redistributions in binary form must reproduce the above *
16 * copyright notice, this list of conditions and the *
17 * following disclaimer in the documentation and/or other *
18 * materials provided with the distribution. *
19 * *
20 * * Neither the name of the WIDE Project or NICT nor the *
21 * names of its contributors may be used to endorse or *
22 * promote products derived from this software without *
23 * specific prior written permission of WIDE Project and *
24 * NICT. *
25 * *
26 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED *
27 * WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A *
28 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR *
29 * ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT *
30 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS *
31 * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR *
32 * TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF *
33 * ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. *
34 *********************************************************************************************************/
35
36 #include "fdproto-internal.h"
37
38 #include <stdarg.h>
39
40 pthread_mutex_t fd_log_lock = PTHREAD_MUTEX_INITIALIZER;
41 pthread_key_t fd_log_thname;
42 int fd_g_debug_lvl = FD_LOG_NOTICE;
43
44 static void fd_internal_logger( int, const char *, va_list );
45 static int use_colors = 0; /* 0: not init, 1: yes, 2: no */
46
47 /* These may be used to pass specific debug requests via the command-line parameters */
48 char * fd_debug_one_function = NULL;
49 char * fd_debug_one_file = NULL;
50
51 /* Useless function, only to ease setting up a breakpoint in gdb (break fd_breakhere) -- use TRACE_HERE */
52 int fd_breaks = 0;
fd_breakhere(void)53 int fd_breakhere(void) { return ++fd_breaks; }
54
55 /* Allow passing of the log and debug information from base stack to extensions */
56 void (*fd_logger)( int loglevel, const char * format, va_list args ) = fd_internal_logger;
57
58 /* Register an external call back for tracing and debug */
fd_log_handler_register(void (* logger)(int loglevel,const char * format,va_list args))59 int fd_log_handler_register( void (*logger)(int loglevel, const char * format, va_list args) )
60 {
61 CHECK_PARAMS( logger );
62
63 if ( fd_logger != fd_internal_logger )
64 {
65 return EALREADY; /* only one registration allowed */
66 }
67 else
68 {
69 fd_logger = logger;
70 }
71
72 return 0;
73 }
74
75 /* Implement a simple reset function here */
fd_log_handler_unregister(void)76 int fd_log_handler_unregister ( void )
77 {
78 fd_logger = fd_internal_logger;
79 return 0; /* Successfull in all cases. */
80 }
81
fd_cleanup_mutex_silent(void * mutex)82 static void fd_cleanup_mutex_silent( void * mutex )
83 {
84 (void)pthread_mutex_unlock((pthread_mutex_t *)mutex);
85 }
86
87
fd_internal_logger(int printlevel,const char * format,va_list ap)88 static void fd_internal_logger( int printlevel, const char *format, va_list ap )
89 {
90 char buf[25];
91
92 /* Do we need to trace this ? */
93 if (printlevel < fd_g_debug_lvl)
94 return;
95
96 /* add timestamp */
97 printf("%s ", fd_log_time(NULL, buf, sizeof(buf),
98 #if (defined(DEBUG) && defined(DEBUG_WITH_META))
99 1, 1
100 #else /* (defined(DEBUG) && defined(DEBUG_WITH_META)) */
101 0, 0
102 #endif /* (defined(DEBUG) && defined(DEBUG_WITH_META)) */
103 ));
104 /* Use colors on stdout ? */
105 if (!use_colors) {
106 if (isatty(STDOUT_FILENO))
107 use_colors = 1;
108 else
109 use_colors = 2;
110 }
111
112 switch(printlevel) {
113 case FD_LOG_ANNOYING: printf("%s A ", (use_colors == 1) ? "\e[0;37m" : ""); break;
114 case FD_LOG_DEBUG: printf("%s DBG ", (use_colors == 1) ? "\e[0;37m" : ""); break;
115 case FD_LOG_INFO: printf("%sINFO ", (use_colors == 1) ? "\e[1;37m" : ""); break;
116 case FD_LOG_NOTICE: printf("%sNOTI ", (use_colors == 1) ? "\e[1;37m" : ""); break;
117 case FD_LOG_ERROR: printf("%sERROR ", (use_colors == 1) ? "\e[0;31m" : ""); break;
118 case FD_LOG_FATAL: printf("%sFATAL! ", (use_colors == 1) ? "\e[0;31m" : ""); break;
119 default: printf("%s ??? ", (use_colors == 1) ? "\e[0;31m" : "");
120 }
121 vprintf(format, ap);
122 if (use_colors == 1)
123 printf("\e[00m");
124 printf("\n");
125
126 fflush(stdout);
127 }
128
129 /* Log a debug message */
fd_log(int loglevel,const char * format,...)130 void fd_log ( int loglevel, const char * format, ... )
131 {
132 va_list ap;
133
134 (void)pthread_mutex_lock(&fd_log_lock);
135
136 pthread_cleanup_push(fd_cleanup_mutex_silent, &fd_log_lock);
137
138 va_start(ap, format);
139 fd_logger(loglevel, format, ap);
140 va_end(ap);
141
142 pthread_cleanup_pop(0);
143
144 (void)pthread_mutex_unlock(&fd_log_lock);
145 }
146
147 /* Log a debug message */
fd_log_va(int loglevel,const char * format,va_list args)148 void fd_log_va ( int loglevel, const char * format, va_list args )
149 {
150 (void)pthread_mutex_lock(&fd_log_lock);
151
152 pthread_cleanup_push(fd_cleanup_mutex_silent, &fd_log_lock);
153 fd_logger(loglevel, format, args);
154 pthread_cleanup_pop(0);
155
156 (void)pthread_mutex_unlock(&fd_log_lock);
157 }
158
159 /* Function to set the thread's friendly name */
fd_log_threadname(const char * name)160 void fd_log_threadname ( const char * name )
161 {
162 void * val = NULL;
163
164 TRACE_ENTRY("%p(%s)", name, name?:"/");
165
166 /* First, check if a value is already assigned to the current thread */
167 val = pthread_getspecific(fd_log_thname);
168 if (TRACE_BOOL(ANNOYING)) {
169 if (val) {
170 fd_log_debug("(Thread '%s' renamed to '%s')", (char *)val, name?:"(nil)");
171 } else {
172 fd_log_debug("(Thread %p named '%s')", (void *)pthread_self(), name?:"(nil)");
173 }
174 }
175 if (val != NULL) {
176 free(val);
177 }
178
179 /* Now create the new string */
180 if (name == NULL) {
181 CHECK_POSIX_DO( pthread_setspecific(fd_log_thname, NULL), /* continue */);
182 return;
183 }
184
185 CHECK_MALLOC_DO( val = strdup(name), return );
186
187 CHECK_POSIX_DO( pthread_setspecific(fd_log_thname, val), /* continue */);
188 return;
189 }
190
191 /* Write time into a buffer */
fd_log_time(struct timespec * ts,char * buf,size_t len,int incl_date,int incl_ms)192 char * fd_log_time ( struct timespec * ts, char * buf, size_t len, int incl_date, int incl_ms )
193 {
194 int ret;
195 size_t offset = 0;
196 struct timespec tp;
197 struct tm tm;
198
199 /* Get current time */
200 if (!ts) {
201 ret = clock_gettime(CLOCK_REALTIME, &tp);
202 if (ret != 0) {
203 snprintf(buf, len, "%s", strerror(ret));
204 return buf;
205 }
206 ts = &tp;
207 }
208
209 offset += strftime(buf + offset, len - offset, incl_date?"%D,%T":"%T", localtime_r( &ts->tv_sec , &tm ));
210 if (incl_ms)
211 offset += snprintf(buf + offset, len - offset, ".%6.6ld", ts->tv_nsec / 1000);
212
213 return buf;
214 }
215
216
217 static size_t sys_mempagesz = 0;
218
get_mempagesz(void)219 static size_t get_mempagesz(void) {
220 if (!sys_mempagesz) {
221 sys_mempagesz = sysconf(_SC_PAGESIZE); /* We alloc buffer by memory pages for efficiency. This can be readjusted if too memory consuming */
222 if (sys_mempagesz <= 0)
223 sys_mempagesz = 256; /* default size if above call failed */
224 }
225 return sys_mempagesz;
226 }
227
228
229 /* Helper function for fd_*_dump. Prints the format string from 'offset' into '*buf', extends if needed. The location of buf can be updated by this function. */
fd_dump_extend(char ** buf,size_t * len,size_t * offset,const char * format,...)230 char * fd_dump_extend(char ** buf, size_t *len, size_t *offset, const char * format, ... )
231 {
232 va_list ap;
233 int to_write;
234 size_t o = 0;
235 size_t mempagesz = get_mempagesz();
236
237 /* we do not TRACE_ENTRY this one on purpose */
238
239 CHECK_PARAMS_DO(buf && len, return NULL);
240
241 if (*buf == NULL) {
242 CHECK_MALLOC_DO(*buf = malloc(mempagesz), return NULL);
243 *len = mempagesz;
244 }
245
246 if (offset)
247 o = *offset;
248
249 va_start(ap, format);
250 to_write = vsnprintf(*buf + o, *len - o, format, ap);
251 va_end(ap);
252
253 if (to_write + o >= *len) {
254 /* There was no room in the buffer, we extend and redo */
255 size_t new_len = (((to_write + o) / mempagesz) + 1) * mempagesz;
256 CHECK_MALLOC_DO(*buf = realloc(*buf, new_len), return NULL);
257 *len = new_len;
258
259 va_start(ap, format);
260 to_write = vsnprintf(*buf + o, *len - o, format, ap);
261 va_end(ap);
262 }
263
264 if (offset)
265 *offset += to_write;
266
267 return *buf;
268 }
269
fd_dump_extend_hexdump(char ** buf,size_t * len,size_t * offset,uint8_t * data,size_t datalen,size_t trunc,size_t wrap)270 char * fd_dump_extend_hexdump(char ** buf, size_t *len, size_t *offset, uint8_t *data, size_t datalen, size_t trunc, size_t wrap )
271 {
272 int truncated = 0;
273 size_t towrite = 0;
274 size_t o = 0;
275 int i;
276 char * p;
277 size_t mempagesz = get_mempagesz();
278 #define TRUNK_MARK "[...]"
279
280 CHECK_PARAMS_DO(buf && len && data, return NULL);
281
282 if (trunc && (datalen > trunc)) {
283 datalen = trunc;
284 truncated = 1;
285 }
286
287 towrite = datalen * 2;
288
289 if (wrap)
290 towrite += datalen / wrap; /* add 1 '\n' every wrap byte */
291
292 if (truncated)
293 towrite += CONSTSTRLEN(TRUNK_MARK);
294
295
296 if (offset)
297 o = *offset;
298
299 if (*buf == NULL) {
300 /* Directly allocate the size we need */
301 *len = (((towrite + o) / mempagesz) + 1 ) * mempagesz;
302 CHECK_MALLOC_DO(*buf = malloc(*len), return NULL);
303 } else if ((towrite + o) >= *len) {
304 /* There is no room in the buffer, we extend and redo */
305 size_t new_len = (((towrite + o) / mempagesz) + 1) * mempagesz;
306 CHECK_MALLOC_DO(*buf = realloc(*buf, new_len), return NULL);
307 *len = new_len;
308 }
309
310 p = *buf + o;
311 for (i = 0; i < datalen; i++) {
312 sprintf(p, "%02hhX", data[i]);
313 p+=2;
314 if ((wrap) && ((i+1) % wrap == 0)) {
315 *p++='\n'; *p ='\0'; /* we want to ensure the buffer is always 0-terminated */
316 }
317 }
318
319 if (truncated)
320 memcpy(p, TRUNK_MARK, CONSTSTRLEN(TRUNK_MARK));
321
322 if (offset)
323 *offset += towrite;
324
325 return *buf;
326 }
327