1 /*
2  * Cisco router simulation platform.
3  * Copyright (c) 2005,2006 Christophe Fillot.  All rights reserved.
4  *
5  * Utility functions.
6  */
7 
8 #include "dynamips_common.h"
9 
10 #define _GNU_SOURCE
11 #include <stdio.h>
12 #include <stdlib.h>
13 #include <string.h>
14 #include <stdarg.h>
15 #include <unistd.h>
16 #include <time.h>
17 #include <signal.h>
18 #include <sys/time.h>
19 #include <sys/ioctl.h>
20 #include <sys/types.h>
21 #include <sys/stat.h>
22 #include <sys/mman.h>
23 #include <sys/socket.h>
24 #include <arpa/inet.h>
25 #include <netdb.h>
26 #include <fcntl.h>
27 #include <errno.h>
28 #include <assert.h>
29 #ifdef __CYGWIN__
30 #include <malloc.h>
31 #endif
32 
33 #include "utils.h"
34 
35 extern FILE *log_file;
36 
37 /* Add an element to a list */
m_list_add(m_list_t ** head,void * data)38 m_list_t *m_list_add(m_list_t **head,void *data)
39 {
40    m_list_t *item;
41 
42    if ((item = malloc(sizeof(*item))) != NULL) {
43       item->data = data;
44       item->next = *head;
45       *head = item;
46    }
47 
48    return item;
49 }
50 
51 /* Dynamic sprintf */
dyn_sprintf(const char * fmt,...)52 char *dyn_sprintf(const char *fmt,...)
53 {
54    int n,size = 512;
55    va_list ap;
56    char *p,*p2;
57 
58    if ((p = malloc(size)) == NULL) {
59       perror("dyn_sprintf: malloc");
60       return NULL;
61    }
62 
63    for(;;)
64    {
65       /* Try to print in the allocated space */
66       va_start(ap,fmt);
67       n = vsnprintf(p,size,fmt,ap);
68       va_end(ap);
69 
70       /* If that worked, return the string */
71       if ((n > -1) && (n < size))
72          return p;
73 
74       /* Else try again with more space. */
75       if (n > -1)
76          size = n + 1;
77       else
78          size *= 2;
79 
80       if ((p2 = realloc(p,size)) == NULL) {
81          perror("dyn_sprintf: realloc");
82          free(p);
83          return NULL;
84       }
85 
86       p = p2;
87    }
88 }
89 
90 /* Split a string */
m_strsplit(char * str,char delim,char ** array,int max_count)91 int m_strsplit(char *str,char delim,char **array,int max_count)
92 {
93    int i,pos = 0;
94    size_t len;
95    char *ptr;
96 
97    for(i=0;i<max_count;i++)
98       array[i] = NULL;
99 
100    do {
101       if (pos == max_count)
102          goto error;
103 
104       ptr = strchr(str,delim);
105       if (!ptr)
106          ptr = str + strlen(str);
107 
108       len = ptr - str;
109 
110       if (!(array[pos] = malloc(len+1)))
111          goto error;
112 
113       memcpy(array[pos],str,len);
114       array[pos][len] = 0;
115 
116       str = ptr + 1;
117       pos++;
118    }while(*ptr);
119 
120    return(pos);
121 
122  error:
123    for(i=0;i<max_count;i++)
124       free(array[i]);
125    return(-1);
126 }
127 
128 /* Tokenize a string */
m_strtok(char * str,char delim,char ** array,int max_count)129 int m_strtok(char *str,char delim,char **array,int max_count)
130 {
131    int i,pos = 0;
132    size_t len;
133    char *ptr;
134 
135    for(i=0;i<max_count;i++)
136       array[i] = NULL;
137 
138    do {
139       if (pos == max_count)
140          goto error;
141 
142       ptr = strchr(str,delim);
143       if (!ptr)
144          ptr = str + strlen(str);
145 
146       len = ptr - str;
147 
148       if (!(array[pos] = malloc(len+1)))
149          goto error;
150 
151       memcpy(array[pos],str,len);
152       array[pos][len] = 0;
153 
154       while(*ptr == delim)
155          ptr++;
156 
157       str = ptr;
158       pos++;
159    }while(*ptr);
160 
161    return(pos);
162 
163  error:
164    for(i=0;i<max_count;i++)
165       free(array[i]);
166    return(-1);
167 }
168 
169 /* Quote a string */
m_strquote(char * buffer,size_t buf_len,char * str)170 char *m_strquote(char *buffer,size_t buf_len,char *str)
171 {
172    char *p;
173 
174    if (!(p = strpbrk(str," \t\"'")))
175       return str;
176 
177    snprintf(buffer,buf_len,"\"%s\"",str);
178    return buffer;
179 }
180 
181 /*
182  * Decode from hex.
183  *
184  * hex to raw bytes, returning count of bytes.
185  * maxlen limits output buffer size.
186  */
hex_decode(unsigned char * out,const unsigned char * in,int maxlen)187 int hex_decode(unsigned char *out,const unsigned char *in,int maxlen)
188 {
189 #define BAD -1
190    static const char hexval[] = {
191       BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD,
192       BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD,
193       BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD,
194         0,  1,  2,  3,   4,  5,  6,  7,   8,  9,BAD,BAD, BAD,BAD,BAD,BAD,
195       BAD, 10, 11, 12,  13, 14, 15,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD,
196       BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD,
197       BAD, 10, 11, 12,  13, 14, 15,BAD, BAD,BAD,BAD,BAD, BAD,BAD,BAD,BAD
198    };
199    int len = 0;
200    int empty = TRUE;
201 
202    for(;len < maxlen;) {
203       if (*in > sizeof(hexval) || hexval[*in] == BAD)
204          break;
205 
206       if (empty) {
207          *out = (unsigned char)hexval[*in] << 4;
208       }
209       else {
210          *out |= (unsigned char)hexval[*in];
211          ++out;
212          ++len;
213       }
214       ++in;
215       empty = !empty;
216    }
217 
218    return(len);
219 }
220 
221 /* Ugly function that dumps a structure in hexa and ascii. */
mem_dump(FILE * f_output,u_char * pkt,u_int len)222 void mem_dump(FILE *f_output,u_char *pkt,u_int len)
223 {
224    u_int x,i = 0, tmp;
225 
226    while (i < len)
227    {
228       if ((len - i) > 16)
229          x = 16;
230       else x = len - i;
231 
232       fprintf(f_output,"%4.4x: ",i);
233 
234       for (tmp=0;tmp<x;tmp++)
235          fprintf(f_output,"%2.2x ",pkt[i+tmp]);
236       for (tmp=x;tmp<16;tmp++) fprintf(f_output,"   ");
237 
238       for (tmp=0;tmp<x;tmp++) {
239          char c = pkt[i+tmp];
240 
241          if (((c >= 'A') && (c <= 'Z')) ||
242              ((c >= 'a') && (c <= 'z')) ||
243              ((c >= '0') && (c <= '9')))
244             fprintf(f_output,"%c",c);
245          else
246             fputs(".",f_output);
247       }
248 
249       i += x;
250       fprintf(f_output,"\n");
251    }
252 
253    fprintf(f_output,"\n");
254    fflush(f_output);
255 }
256 
257 /* Logging function */
m_flog(FILE * fd,char * module,char * fmt,va_list ap)258 void m_flog(FILE *fd,char *module,char *fmt,va_list ap)
259 {
260    struct timeval now;
261    struct tm tmn;
262    time_t ct;
263    char buf[256];
264 
265    if (fd != NULL) {
266       gettimeofday(&now,0);
267       ct = now.tv_sec;
268       localtime_r(&ct,&tmn);
269 
270       strftime(buf,sizeof(buf),"%b %d %H:%M:%S",&tmn);
271 
272       fprintf(fd,"%s.%03ld %s: ",buf,(long)now.tv_usec/1000,module);
273       vfprintf(fd,fmt,ap);
274       fflush(fd);
275    }
276 }
277 
278 /* Logging function */
m_log(char * module,char * fmt,...)279 void m_log(char *module,char *fmt,...)
280 {
281    va_list ap;
282 
283    va_start(ap,fmt);
284    m_flog(log_file,module,fmt,ap);
285    va_end(ap);
286 }
287 
288 /* Write an array of string to a logfile */
m_flog_str_array(FILE * fd,int count,char * str[])289 void m_flog_str_array(FILE *fd,int count,char *str[])
290 {
291    int i;
292 
293    for(i=0;i<count;i++)
294       fprintf(fd,"%s ",str[i]);
295 
296    fprintf(fd,"\n");
297    fflush(fd);
298 }
299 
300 /* Returns a line from specified file (remove trailing '\n') */
m_fgets(char * buffer,int size,FILE * fd)301 char *m_fgets(char *buffer,int size,FILE *fd)
302 {
303    int len;
304 
305    buffer[0] = '\0';
306    fgets(buffer,size,fd);
307 
308    if ((len = strlen(buffer)) == 0)
309       return NULL;
310 
311    /* remove trailing '\n' */
312    if (buffer[len-1] == '\n')
313       buffer[len-1] = '\0';
314 
315    return buffer;
316 }
317 
318 /* Read a file and returns it in a buffer */
m_read_file(const char * filename,u_char ** buffer,size_t * length)319 int m_read_file(const char *filename,u_char **buffer,size_t *length)
320 {
321    FILE *fd;
322    long len;
323 
324    // open
325    fd = fopen(filename,"rb");
326    if (fd == NULL) {
327       return(-1);
328    }
329 
330    // len
331    fseek(fd, 0, SEEK_END);
332    len = ftell(fd);
333    fseek(fd, 0, SEEK_SET);
334    if (len < 0 || ferror(fd)) {
335       fclose(fd);
336       return(-1);
337    }
338 
339    if (length) {
340       *length = (size_t)len;
341    }
342 
343     // data
344    if (buffer) {
345       *buffer = (u_char *)malloc((size_t)len);
346       if (*buffer == NULL || fread(*buffer, (size_t)len, 1, fd) != 1) {
347          free(*buffer);
348          *buffer = NULL;
349          fclose(fd);
350          return(-1);
351       }
352    }
353 
354    // close
355    fclose(fd);
356    return(0);
357 }
358 
359 /* Allocate aligned memory */
m_memalign(size_t boundary,size_t size)360 void *m_memalign(size_t boundary,size_t size)
361 {
362    void *p;
363 
364 #if defined(__linux__) || HAS_POSIX_MEMALIGN
365    if (posix_memalign((void *)&p,boundary,size))
366 #else
367 #if defined(__CYGWIN__) || defined(SUNOS)
368    if (!(p = memalign(boundary,size)))
369 #else
370    if (!(p = malloc(size)))
371 #endif
372 #endif
373       return NULL;
374 
375    assert(((m_iptr_t)p & (boundary-1)) == 0);
376    return p;
377 }
378 
379 /* Block specified signal for calling thread */
m_signal_block(int sig)380 int m_signal_block(int sig)
381 {
382    sigset_t sig_mask;
383    sigemptyset(&sig_mask);
384    sigaddset(&sig_mask,sig);
385    return(pthread_sigmask(SIG_BLOCK,&sig_mask,NULL));
386 }
387 
388 /* Unblock specified signal for calling thread */
m_signal_unblock(int sig)389 int m_signal_unblock(int sig)
390 {
391    sigset_t sig_mask;
392    sigemptyset(&sig_mask);
393    sigaddset(&sig_mask,sig);
394    return(pthread_sigmask(SIG_UNBLOCK,&sig_mask,NULL));
395 }
396 
397 /* Set non-blocking mode on a file descriptor */
m_fd_set_non_block(int fd)398 int m_fd_set_non_block(int fd)
399 {
400    int flags;
401 
402    if ((flags = fcntl(fd,F_GETFL,0)) < 0)
403       return(-1);
404 
405    return(fcntl(fd,F_SETFL, flags | O_NONBLOCK));
406 }
407 
408 /* Sync a memory zone */
memzone_sync(void * addr,size_t len)409 int memzone_sync(void *addr, size_t len)
410 {
411    return(msync(addr, len, MS_SYNC));
412 }
413 
414 /* Sync all mappings of a memory zone */
memzone_sync_all(void * addr,size_t len)415 int memzone_sync_all(void *addr, size_t len)
416 {
417    return(msync(addr, len, MS_SYNC | MS_INVALIDATE));
418 }
419 
420 /* Unmap a memory zone */
memzone_unmap(void * addr,size_t len)421 int memzone_unmap(void *addr, size_t len)
422 {
423    return(munmap(addr, len));
424 }
425 
426 /* Return a memory zone or NULL on error */
mmap_or_null(void * addr,size_t length,int prot,int flags,int fd,off_t offset)427 static void *mmap_or_null(void *addr, size_t length, int prot, int flags, int fd, off_t offset)
428 {
429    void *ptr;
430 
431    if ((ptr = mmap(addr, length, prot, flags, fd, offset)) == MAP_FAILED)
432       return(NULL);
433 
434    return(ptr);
435 }
436 
437 /* Map a memory zone as an executable area */
memzone_map_exec_area(size_t len)438 u_char *memzone_map_exec_area(size_t len)
439 {
440    return(mmap_or_null(NULL,len,PROT_EXEC|PROT_READ|PROT_WRITE,MAP_SHARED|MAP_ANONYMOUS,-1,(off_t)0));
441 }
442 
443 /* Map a memory zone from a file */
memzone_map_file(int fd,size_t len)444 u_char *memzone_map_file(int fd,size_t len)
445 {
446    return(mmap_or_null(NULL,len,PROT_READ|PROT_WRITE,MAP_SHARED,fd,(off_t)0));
447 }
448 
449 /* Map a memory zone from a ro file */
memzone_map_file_ro(int fd,size_t len)450 u_char *memzone_map_file_ro(int fd,size_t len)
451 {
452    return(mmap_or_null(NULL,len,PROT_READ,MAP_PRIVATE,fd,(off_t)0));
453 }
454 
455 /* Map a memory zone from a file, with copy-on-write (COW) */
memzone_map_cow_file(int fd,size_t len)456 u_char *memzone_map_cow_file(int fd,size_t len)
457 {
458    return(mmap_or_null(NULL,len,PROT_READ|PROT_WRITE,MAP_PRIVATE,fd,(off_t)0));
459 }
460 
461 /* Create a file to serve as a memory zone */
memzone_create_file(char * filename,size_t len,u_char ** ptr)462 int memzone_create_file(char *filename,size_t len,u_char **ptr)
463 {
464    int fd;
465 
466    if ((fd = open(filename,O_CREAT|O_RDWR,S_IRWXU)) == -1) {
467       perror("memzone_create_file: open");
468       return(-1);
469    }
470 
471    if (ftruncate(fd,len) == -1) {
472       perror("memzone_create_file: ftruncate");
473       close(fd);
474       return(-1);
475    }
476 
477    *ptr = memzone_map_file(fd,len);
478 
479    if (!*ptr) {
480       close(fd);
481       fd = -1;
482    }
483 
484    return(fd);
485 }
486 
487 /* Open a file to serve as a COW memory zone */
memzone_open_cow_file(char * filename,size_t len,u_char ** ptr)488 int memzone_open_cow_file(char *filename,size_t len,u_char **ptr)
489 {
490    int fd;
491 
492    if ((fd = open(filename,O_RDONLY,S_IRWXU)) == -1) {
493       perror("memzone_open_file: open");
494       return(-1);
495    }
496 
497    *ptr = memzone_map_cow_file(fd,len);
498 
499    if (!*ptr) {
500       close(fd);
501       fd = -1;
502    }
503 
504    return(fd);
505 }
506 
507 /* Open a file and map it in memory */
memzone_open_file(char * filename,u_char ** ptr,off_t * fsize)508 int memzone_open_file(char *filename,u_char **ptr,off_t *fsize)
509 {
510    struct stat fprop;
511    int fd;
512 
513    if ((fd = open(filename,O_RDWR,S_IRWXU)) == -1)
514       return(-1);
515 
516    if (fstat(fd,&fprop) == -1)
517       goto err_fstat;
518 
519    *fsize = fprop.st_size;
520    if (!(*ptr = memzone_map_file(fd,*fsize)))
521       goto err_mmap;
522 
523    return(fd);
524 
525  err_mmap:
526  err_fstat:
527    close(fd);
528    return(-1);
529 }
530 
memzone_open_file_ro(char * filename,u_char ** ptr,off_t * fsize)531 int memzone_open_file_ro(char *filename,u_char **ptr,off_t *fsize)
532 {
533    struct stat fprop;
534    int fd;
535 
536    if ((fd = open(filename,O_RDONLY,S_IRWXU)) == -1)
537       return(-1);
538 
539    if (fstat(fd,&fprop) == -1)
540       goto err_fstat;
541 
542    *fsize = fprop.st_size;
543    if (!(*ptr = memzone_map_file_ro(fd,*fsize)))
544       goto err_mmap;
545 
546    return(fd);
547 
548  err_mmap:
549  err_fstat:
550    close(fd);
551    return(-1);
552 }
553 
554 /* Compute NVRAM checksum */
nvram_cksum(m_uint16_t * ptr,size_t count)555 m_uint16_t nvram_cksum(m_uint16_t *ptr,size_t count)
556 {
557    m_uint32_t sum = 0;
558 
559    while(count > 1) {
560       sum = sum + ntohs(*ptr);
561       ptr++;
562       count -= sizeof(m_uint16_t);
563    }
564 
565    if (count > 0)
566       sum = sum + ((ntohs(*ptr) & 0xFF) << 8);
567 
568    while(sum>>16)
569       sum = (sum & 0xffff) + (sum >> 16);
570 
571    return(~sum);
572 }
573 
574 /* Byte-swap a memory block */
mem_bswap32(void * ptr,size_t len)575 void mem_bswap32(void *ptr,size_t len)
576 {
577    m_uint32_t *p _not_aligned = ptr;
578    size_t count = len >> 2;
579    int i;
580 
581    for(i=0;i<count;i++,p++)
582       *p = swap32(*p);
583 }
584 
585 /* Reverse a byte */
m_reverse_u8(m_uint8_t val)586 m_uint8_t m_reverse_u8(m_uint8_t val)
587 {
588    m_uint8_t res = 0;
589    int i;
590 
591    for(i=0;i<8;i++)
592       if (val & (1 << i))
593          res |= 1 << (7 - i);
594 
595    return(res);
596 }
597 
598 /* Generate a pseudo random block of data */
m_randomize_block(m_uint8_t * buf,size_t len)599 void m_randomize_block(m_uint8_t *buf,size_t len)
600 {
601    int i;
602 
603    for(i=0;i<len;i++)
604       buf[i] = rand() & 0xFF;
605 }
606 
607 /* Free an FD pool */
fd_pool_free(fd_pool_t * pool)608 void fd_pool_free(fd_pool_t *pool)
609 {
610    fd_pool_t *p,*next;
611    int i;
612 
613    for(p=pool;p;p=next) {
614       next = p->next;
615 
616       for(i=0;i<FD_POOL_MAX;i++) {
617          if (p->fd[i] != -1) {
618             shutdown(p->fd[i],2);
619             close(p->fd[i]);
620          }
621       }
622 
623       if (pool != p)
624          free(p);
625    }
626 }
627 
628 /* Initialize an empty pool */
fd_pool_init(fd_pool_t * pool)629 void fd_pool_init(fd_pool_t *pool)
630 {
631    int i;
632 
633    for(i=0;i<FD_POOL_MAX;i++)
634       pool->fd[i] = -1;
635 
636    pool->next = NULL;
637 }
638 
639 /* Get a free slot for a FD in a pool */
fd_pool_get_free_slot(fd_pool_t * pool,int ** slot)640 int fd_pool_get_free_slot(fd_pool_t *pool,int **slot)
641 {
642    fd_pool_t *p;
643    int i;
644 
645    for(p=pool;p;p=p->next) {
646       for(i=0;i<FD_POOL_MAX;i++) {
647          if (p->fd[i] == -1) {
648             *slot = &p->fd[i];
649             return(0);
650          }
651       }
652    }
653 
654    /* No free slot, allocate a new pool */
655    if (!(p = malloc(sizeof(*p))))
656       return(-1);
657 
658    fd_pool_init(p);
659    *slot = &p->fd[0];
660 
661    p->next = pool->next;
662    pool->next = p;
663    return(0);
664 }
665 
666 /* Fill a FD set and get the maximum FD in order to use with select */
fd_pool_set_fds(fd_pool_t * pool,fd_set * fds)667 int fd_pool_set_fds(fd_pool_t *pool,fd_set *fds)
668 {
669    fd_pool_t *p;
670    int i,max_fd = -1;
671 
672    for(p=pool;p;p=p->next)
673       for(i=0;i<FD_POOL_MAX;i++) {
674          if (p->fd[i] != -1) {
675             FD_SET(p->fd[i],fds);
676 
677             if (p->fd[i] > max_fd)
678                max_fd = p->fd[i];
679          }
680       }
681 
682    return(max_fd);
683 }
684 
685 /* Send a buffer to all FDs of a pool */
fd_pool_send(fd_pool_t * pool,void * buffer,size_t len,int flags)686 int fd_pool_send(fd_pool_t *pool,void *buffer,size_t len,int flags)
687 {
688    fd_pool_t *p;
689    ssize_t res;
690    int i,err;
691 
692    for(p=pool,err=0;p;p=p->next)
693       for(i=0;i<FD_POOL_MAX;i++) {
694          if (p->fd[i] == -1)
695             continue;
696 
697          res = send(p->fd[i],buffer,len,flags);
698 
699          if (res != len) {
700             shutdown(p->fd[i],2);
701             close(p->fd[i]);
702             p->fd[i] = -1;
703             err++;
704          }
705       }
706 
707    return(err);
708 }
709 
710 /* Call a function for each FD having incoming data */
fd_pool_check_input(fd_pool_t * pool,fd_set * fds,void (* cbk)(int * fd_slot,void * opt),void * opt)711 int fd_pool_check_input(fd_pool_t *pool,fd_set *fds,
712                         void (*cbk)(int *fd_slot,void *opt),void *opt)
713 {
714    fd_pool_t *p;
715    int i,count;
716 
717    for(p=pool,count=0;p;p=p->next)
718       for(i=0;i<FD_POOL_MAX;i++) {
719          if ((p->fd[i] != -1) && FD_ISSET(p->fd[i],fds)) {
720             cbk(&p->fd[i],opt);
721             count++;
722          }
723       }
724 
725    return(count);
726 }
727 
728 /* Equivalent to fprintf, but for a posix fd */
fd_printf(int fd,int flags,char * fmt,...)729 ssize_t fd_printf(int fd,int flags,char *fmt,...)
730 {
731    char buffer[2048];
732    va_list ap;
733 
734    va_start(ap,fmt);
735    vsnprintf(buffer,sizeof(buffer),fmt,ap);
736    va_end(ap);
737 
738    return(send(fd,buffer,strlen(buffer),flags));
739 }
740