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