dynamips/trunk/utils.c

/*  
 * Cisco router simulation platform.
 * Copyright (c) 2005,2006 Christophe Fillot.  All rights reserved.
 *
 * Utility functions.
 */

#define _GNU_SOURCE
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <stdarg.h>
#include <unistd.h>
#include <time.h>
#include <signal.h>
#include <sys/time.h>
#include <sys/ioctl.h>
#include <sys/types.h>
#include <sys/stat.h>
#include <sys/socket.h>
#include <arpa/inet.h>
#include <netdb.h>
#include <fcntl.h>
#include <errno.h>
#include <assert.h>
#ifdef __CYGWIN__
#include <malloc.h>
#endif

#include "utils.h"

extern FILE *log_file;

/* Add an element to a list */
m_list_t *m_list_add(m_list_t **head,void *data)
{
   m_list_t *item;

   if ((item = malloc(sizeof(*item))) != NULL) {
      item->data = data;
      item->next = *head;
      *head = item;
   }

   return item;
}

/* Dynamic sprintf */
char *dyn_sprintf(const char *fmt,...)
{
   int n,size = 512;
   va_list ap;
   char *p,*p2;

   if ((p = malloc(size)) == NULL) {
      perror("dyn_sprintf: malloc");
      return NULL;
   }

   for(;;)
   {
      /* Try to print in the allocated space */
      va_start(ap,fmt);
      n = vsnprintf(p,size,fmt,ap);
      va_end(ap);

      /* If that worked, return the string */
      if ((n > -1) && (n < size))
         return p;

      /* Else try again with more space. */
      if (n > -1)
         size = n + 1;
      else
         size *= 2;

      if ((p2 = realloc(p,size)) == NULL) {
         perror("dyn_sprintf: realloc");
         free(p);
         return NULL;
      }

      p = p2;
   }
}

/* Split a string */
int m_strsplit(char *str,char delim,char **array,int max_count)
{
   int i,pos = 0;
   size_t len;
   char *ptr;

   for(i=0;i<max_count;i++)
      array[i] = NULL;

   do {
      if (pos == max_count)
         goto error;

      ptr = strchr(str,delim);
      if (!ptr)
         ptr = str + strlen(str);

      len = ptr - str;

      if (!(array[pos] = malloc(len+1)))
         goto error;

      memcpy(array[pos],str,len);
      array[pos][len] = 0;

      str = ptr + 1;
      pos++;
   }while(*ptr);
   
   return(pos);

 error:
   for(i=0;i<max_count;i++)
      free(array[i]);
   return(-1);
}

/* Tokenize a string */
int m_strtok(char *str,char delim,char **array,int max_count)
{
   int i,pos = 0;
   size_t len;
   char *ptr;

   for(i=0;i<max_count;i++)
      array[i] = NULL;

   do {
      if (pos == max_count)
         goto error;

      ptr = strchr(str,delim);
      if (!ptr)
         ptr = str + strlen(str);

      len = ptr - str;

      if (!(array[pos] = malloc(len+1)))
         goto error;

      memcpy(array[pos],str,len);
      array[pos][len] = 0;

      while(*ptr == delim) 
         ptr++;

      str = ptr;
      pos++;
   }while(*ptr);
   
   return(pos);

 error:
   for(i=0;i<max_count;i++)
      free(array[i]);
   return(-1);
}

/* Quote a string */
char *m_strquote(char *buffer,size_t buf_len,char *str)
{
   char *p;
   
   if (!(p = strpbrk(str," \t\"'")))
      return str;

   snprintf(buffer,buf_len,"\"%s\"",str);
   return buffer;
}

/* Ugly function that dumps a structure in hexa and ascii. */
void mem_dump(FILE *f_output,u_char *pkt,u_int len)
{
   u_int x,i = 0, tmp;

   while (i < len)
   {
      if ((len - i) > 16)
         x = 16;
      else x = len - i;

      fprintf(f_output,"%4.4x: ",i);

      for (tmp=0;tmp<x;tmp++)
         fprintf(f_output,"%2.2x ",pkt[i+tmp]);
      for (tmp=x;tmp<16;tmp++) fprintf(f_output,"   ");

      for (tmp=0;tmp<x;tmp++) {
         char c = pkt[i+tmp];

         if (((c >= 'A') && (c <= 'Z')) ||
             ((c >= 'a') && (c <= 'z')) ||
             ((c >= '0') && (c <= '9')))
            fprintf(f_output,"%c",c);
         else
            fputs(".",f_output);
      }

      i += x;
      fprintf(f_output,"\n");
   }

   fprintf(f_output,"\n");
   fflush(f_output);
}

/* Logging function */
void m_flog(FILE *fd,char *module,char *fmt,va_list ap)
{
   struct timeval now;
   static char buf[256];
   time_t ct;

   gettimeofday(&now,0);
   ct = now.tv_sec;
   strftime(buf,sizeof(buf),"%b %d %H:%M:%S",localtime(&ct));
   if (fd) {
      fprintf(fd,"%s.%03ld %s: ",buf,(long)now.tv_usec/1000,module);
      vfprintf(fd,fmt,ap);
      fflush(fd);
   }
}

/* Logging function */
void m_log(char *module,char *fmt,...)
{
   va_list ap;

   va_start(ap,fmt);
   m_flog(log_file,module,fmt,ap);
   va_end(ap);
}

/* Returns a line from specified file (remove trailing '\n') */
char *m_fgets(char *buffer,int size,FILE *fd)
{
   int len;

   buffer[0] = '\0';
   fgets(buffer,size,fd);

   if ((len = strlen(buffer)) == 0)
      return NULL;

   /* remove trailing '\n' */
   if (buffer[len-1] == '\n')
      buffer[len-1] = '\0';

   return buffer;
}

/* Read a file and returns it in a buffer */
ssize_t m_read_file(char *filename,u_char **buffer)
{
   u_char tmp[256],*ptr,*nptr;
   size_t len,tot_len;
   FILE *fd;

   *buffer = ptr = NULL;
   tot_len = 0;

   /* Open file for reading */
   if ((fd = fopen(filename,"r")) == NULL)
      return(-1);

   while((len = fread(tmp,1,sizeof(tmp),fd)) > 0)
   {
      /* Reallocate memory */
      nptr = realloc(ptr,tot_len+len+1);
      if (nptr == NULL) {
         if (ptr) free(ptr);
         fclose(fd);
         return(-1);
      }

      ptr = nptr;

      /* Ok, memory could be allocated */
      memcpy(&ptr[tot_len],tmp,len);
      tot_len += len;
   }

   fclose(fd);
   *buffer = ptr;
   return(tot_len);
}

/* Allocate aligned memory */
void *m_memalign(size_t boundary,size_t size)
{
   void *p;

#if defined(__linux__) || HAS_POSIX_MEMALIGN
   if (posix_memalign((void *)&p,boundary,size))
#else
#if defined(__CYGWIN__) || defined(SUNOS)
   if (!(p = memalign(boundary,size)))
#else
   if (!(p = malloc(size)))    
#endif
#endif
      return NULL;

   assert(((m_iptr_t)p & (boundary-1)) == 0);
   return p;
}

/* Block specified signal for calling thread */
int m_signal_block(int sig)
{
   sigset_t sig_mask;
   sigemptyset(&sig_mask);
   sigaddset(&sig_mask,sig);
   return(pthread_sigmask(SIG_BLOCK,&sig_mask,NULL));
}

/* Unblock specified signal for calling thread */
int m_signal_unblock(int sig)
{
   sigset_t sig_mask;
   sigemptyset(&sig_mask);
   sigaddset(&sig_mask,sig);
   return(pthread_sigmask(SIG_UNBLOCK,&sig_mask,NULL));
}

/* Set non-blocking mode on a file descriptor */
int m_fd_set_non_block(int fd)
{
   int flags;

   if ((flags = fcntl(fd,F_GETFL,0)) < 1)
      return(-1);

   return(fcntl(fd,F_SETFL, flags | O_NONBLOCK));
}

/* Map a memory zone from a file */
u_char *memzone_map_file(int fd,size_t len)
{
   return(mmap(NULL,len,PROT_READ|PROT_WRITE,MAP_SHARED,fd,(off_t)0));
}

/* Map a memory zone from a file, with copy-on-write (COW) */
u_char *memzone_map_cow_file(int fd,size_t len)
{
   return(mmap(NULL,len,PROT_READ|PROT_WRITE,MAP_PRIVATE,fd,(off_t)0));
}

/* Create a file to serve as a memory zone */
int memzone_create_file(char *filename,size_t len,u_char **ptr)
{
   int fd;

   if ((fd = open(filename,O_CREAT|O_RDWR,S_IRWXU)) == -1) {
      perror("memzone_create_file: open");
      return(-1);
   }

   if (ftruncate(fd,len) == -1) {
      perror("memzone_create_file: ftruncate");
      close(fd);
      return(-1);
   }

   *ptr = memzone_map_file(fd,len);

   if (!*ptr) {
      close(fd);
      fd = -1;
   }

   return(fd);
}

/* Open a file to serve as a COW memory zone */
int memzone_open_cow_file(char *filename,size_t len,u_char **ptr)
{
   int fd;

   if ((fd = open(filename,O_RDWR,S_IRWXU)) == -1) {
      perror("memzone_open_file: open");
      return(-1);
   }

   *ptr = memzone_map_cow_file(fd,len);

   if (!*ptr) {
      close(fd);
      fd = -1;
   }

   return(fd);
}

/* Open a file and map it in memory */
int memzone_open_file(char *filename,u_char **ptr,off_t *fsize)
{
   struct stat fprop;
   int fd;

   if ((fd = open(filename,O_RDWR,S_IRWXU)) == -1)
      return(-1);

   if (fstat(fd,&fprop) == -1)
      goto err_fstat;

   *fsize = fprop.st_size;
   if (!(*ptr = memzone_map_file(fd,*fsize)))
      goto err_mmap;
   
   return(fd);

 err_mmap:   
 err_fstat:
   close(fd);
   return(-1);
}

/* Compute NVRAM checksum */
m_uint16_t nvram_cksum(m_uint16_t *ptr,size_t count) 
{
   m_uint32_t sum = 0;

   while(count > 1) {
      sum = sum + ntohs(*ptr);
      ptr++;
      count -= sizeof(m_uint16_t);
   }

   if (count > 0) 
      sum = sum + ((ntohs(*ptr) & 0xFF) << 8); 

   while(sum>>16)
      sum = (sum & 0xffff) + (sum >> 16);

   return(~sum);
}

/* Byte-swap a memory block */
void mem_bswap32(void *ptr,size_t len)
{
   m_uint32_t *p = ptr;
   size_t count = len >> 2;
   int i;

   for(i=0;i<count;i++,p++)
      *p = swap32(*p);
}

/* Reverse a byte */
m_uint8_t m_reverse_u8(m_uint8_t val)
{
   m_uint8_t res = 0;
   int i;

   for(i=0;i<8;i++)
      if (val & (1 << i))
         res |= 1 << (7 - i);
   
   return(res);
}
1	dpavlin	1	/*
2	dpavlin	7	* Cisco router simulation platform.
3	dpavlin	1	* Copyright (c) 2005,2006 Christophe Fillot. All rights reserved.
4			*
5			* Utility functions.
6			*/
7
8			#define _GNU_SOURCE
9			#include <stdio.h>
10			#include <stdlib.h>
11			#include <string.h>
12			#include <stdarg.h>
13			#include <unistd.h>
14			#include <time.h>
15	dpavlin	3	#include <signal.h>
16	dpavlin	1	#include <sys/time.h>
17			#include <sys/ioctl.h>
18			#include <sys/types.h>
19	dpavlin	4	#include <sys/stat.h>
20	dpavlin	1	#include <sys/socket.h>
21			#include <arpa/inet.h>
22			#include <netdb.h>
23			#include <fcntl.h>
24			#include <errno.h>
25			#include <assert.h>
26			#ifdef __CYGWIN__
27			#include <malloc.h>
28			#endif
29
30			#include "utils.h"
31
32			extern FILE *log_file;
33
34			/* Add an element to a list */
35			m_list_t m_list_add(m_list_t head,void data)
36			{
37			m_list_t *item;
38
39			if ((item = malloc(sizeof(*item))) != NULL) {
40			item->data = data;
41			item->next = *head;
42			*head = item;
43			}
44
45			return item;
46			}
47
48			/* Dynamic sprintf */
49			char dyn_sprintf(const char fmt,...)
50			{
51			int n,size = 512;
52			va_list ap;
53			char p,p2;
54
55			if ((p = malloc(size)) == NULL) {
56			perror("dyn_sprintf: malloc");
57			return NULL;
58			}
59
60			for(;;)
61			{
62			/* Try to print in the allocated space */
63			va_start(ap,fmt);
64			n = vsnprintf(p,size,fmt,ap);
65			va_end(ap);
66
67			/* If that worked, return the string */
68			if ((n > -1) && (n < size))
69			return p;
70
71			/* Else try again with more space. */
72			if (n > -1)
73			size = n + 1;
74			else
75			size *= 2;
76
77			if ((p2 = realloc(p,size)) == NULL) {
78			perror("dyn_sprintf: realloc");
79			free(p);
80			return NULL;
81			}
82
83			p = p2;
84			}
85			}
86
87			/* Split a string */
88			int m_strsplit(char str,char delim,char *array,int max_count)
89			{
90			int i,pos = 0;
91			size_t len;
92			char *ptr;
93
94			for(i=0;i<max_count;i++)
95			array[i] = NULL;
96
97			do {
98			if (pos == max_count)
99			goto error;
100
101			ptr = strchr(str,delim);
102			if (!ptr)
103			ptr = str + strlen(str);
104
105			len = ptr - str;
106
107			if (!(array[pos] = malloc(len+1)))
108			goto error;
109
110			memcpy(array[pos],str,len);
111			array[pos][len] = 0;
112
113			str = ptr + 1;
114			pos++;
115			}while(*ptr);
116
117			return(pos);
118
119			error:
120			for(i=0;i<max_count;i++)
121			free(array[i]);
122			return(-1);
123			}
124
125			/* Tokenize a string */
126			int m_strtok(char str,char delim,char *array,int max_count)
127			{
128			int i,pos = 0;
129			size_t len;
130			char *ptr;
131
132			for(i=0;i<max_count;i++)
133			array[i] = NULL;
134
135			do {
136			if (pos == max_count)
137			goto error;
138
139			ptr = strchr(str,delim);
140			if (!ptr)
141			ptr = str + strlen(str);
142
143			len = ptr - str;
144
145			if (!(array[pos] = malloc(len+1)))
146			goto error;
147
148			memcpy(array[pos],str,len);
149			array[pos][len] = 0;
150
151			while(*ptr == delim)
152			ptr++;
153
154			str = ptr;
155			pos++;
156			}while(*ptr);
157
158			return(pos);
159
160			error:
161			for(i=0;i<max_count;i++)
162			free(array[i]);
163			return(-1);
164			}
165
166			/* Quote a string */
167			char m_strquote(char buffer,size_t buf_len,char *str)
168			{
169			char *p;
170
171			if (!(p = strpbrk(str," \t\"'")))
172			return str;
173
174			snprintf(buffer,buf_len,"\"%s\"",str);
175			return buffer;
176			}
177
178			/* Ugly function that dumps a structure in hexa and ascii. */
179			void mem_dump(FILE f_output,u_char pkt,u_int len)
180			{
181			u_int x,i = 0, tmp;
182
183			while (i < len)
184			{
185			if ((len - i) > 16)
186			x = 16;
187			else x = len - i;
188
189			fprintf(f_output,"%4.4x: ",i);
190
191			for (tmp=0;tmp<x;tmp++)
192			fprintf(f_output,"%2.2x ",pkt[i+tmp]);
193			for (tmp=x;tmp<16;tmp++) fprintf(f_output," ");
194
195			for (tmp=0;tmp<x;tmp++) {
196			char c = pkt[i+tmp];
197
198			if (((c >= 'A') && (c <= 'Z')) \|\|
199			((c >= 'a') && (c <= 'z')) \|\|
200			((c >= '0') && (c <= '9')))
201			fprintf(f_output,"%c",c);
202			else
203			fputs(".",f_output);
204			}
205
206			i += x;
207			fprintf(f_output,"\n");
208			}
209
210			fprintf(f_output,"\n");
211	dpavlin	11	fflush(f_output);
212	dpavlin	1	}
213
214			/* Logging function */
215			void m_flog(FILE fd,char module,char *fmt,va_list ap)
216			{
217			struct timeval now;
218			static char buf[256];
219			time_t ct;
220
221			gettimeofday(&now,0);
222			ct = now.tv_sec;
223			strftime(buf,sizeof(buf),"%b %d %H:%M:%S",localtime(&ct));
224			if (fd) {
225			fprintf(fd,"%s.%03ld %s: ",buf,(long)now.tv_usec/1000,module);
226			vfprintf(fd,fmt,ap);
227			fflush(fd);
228			}
229			}
230
231			/* Logging function */
232			void m_log(char module,char fmt,...)
233			{
234			va_list ap;
235
236			va_start(ap,fmt);
237			m_flog(log_file,module,fmt,ap);
238			va_end(ap);
239			}
240
241			/* Returns a line from specified file (remove trailing '\n') */
242			char m_fgets(char buffer,int size,FILE *fd)
243			{
244			int len;
245
246			buffer[0] = '\0';
247			fgets(buffer,size,fd);
248
249			if ((len = strlen(buffer)) == 0)
250			return NULL;
251
252			/* remove trailing '\n' */
253			if (buffer[len-1] == '\n')
254			buffer[len-1] = '\0';
255
256			return buffer;
257			}
258
259			/* Read a file and returns it in a buffer */
260	dpavlin	11	ssize_t m_read_file(char filename,u_char *buffer)
261	dpavlin	1	{
262	dpavlin	11	u_char tmp[256],ptr,nptr;
263	dpavlin	1	size_t len,tot_len;
264			FILE *fd;
265
266			*buffer = ptr = NULL;
267			tot_len = 0;
268
269			/* Open file for reading */
270			if ((fd = fopen(filename,"r")) == NULL)
271			return(-1);
272
273			while((len = fread(tmp,1,sizeof(tmp),fd)) > 0)
274			{
275			/* Reallocate memory */
276			nptr = realloc(ptr,tot_len+len+1);
277			if (nptr == NULL) {
278			if (ptr) free(ptr);
279			fclose(fd);
280			return(-1);
281			}
282
283			ptr = nptr;
284
285			/* Ok, memory could be allocated */
286			memcpy(&ptr[tot_len],tmp,len);
287			tot_len += len;
288			}
289
290			fclose(fd);
291			*buffer = ptr;
292			return(tot_len);
293			}
294
295			/* Allocate aligned memory */
296			void *m_memalign(size_t boundary,size_t size)
297			{
298			void *p;
299
300	dpavlin	11	#if defined(__linux__) \|\| HAS_POSIX_MEMALIGN
301	dpavlin	1	if (posix_memalign((void *)&p,boundary,size))
302			#else
303	dpavlin	3	#if defined(__CYGWIN__) \|\| defined(SUNOS)
304	dpavlin	1	if (!(p = memalign(boundary,size)))
305			#else
306			if (!(p = malloc(size)))
307			#endif
308			#endif
309			return NULL;
310
311			assert(((m_iptr_t)p & (boundary-1)) == 0);
312			return p;
313			}
314
315	dpavlin	3	/* Block specified signal for calling thread */
316			int m_signal_block(int sig)
317			{
318			sigset_t sig_mask;
319			sigemptyset(&sig_mask);
320			sigaddset(&sig_mask,sig);
321			return(pthread_sigmask(SIG_BLOCK,&sig_mask,NULL));
322			}
323
324			/* Unblock specified signal for calling thread */
325			int m_signal_unblock(int sig)
326			{
327			sigset_t sig_mask;
328			sigemptyset(&sig_mask);
329			sigaddset(&sig_mask,sig);
330			return(pthread_sigmask(SIG_UNBLOCK,&sig_mask,NULL));
331			}
332
333			/* Set non-blocking mode on a file descriptor */
334			int m_fd_set_non_block(int fd)
335			{
336			int flags;
337
338			if ((flags = fcntl(fd,F_GETFL,0)) < 1)
339			return(-1);
340
341			return(fcntl(fd,F_SETFL, flags \| O_NONBLOCK));
342			}
343	dpavlin	4
344			/* Map a memory zone from a file */
345			u_char *memzone_map_file(int fd,size_t len)
346			{
347			return(mmap(NULL,len,PROT_READ\|PROT_WRITE,MAP_SHARED,fd,(off_t)0));
348			}
349
350			/* Map a memory zone from a file, with copy-on-write (COW) */
351			u_char *memzone_map_cow_file(int fd,size_t len)
352			{
353			return(mmap(NULL,len,PROT_READ\|PROT_WRITE,MAP_PRIVATE,fd,(off_t)0));
354			}
355
356			/* Create a file to serve as a memory zone */
357			int memzone_create_file(char filename,size_t len,u_char *ptr)
358			{
359			int fd;
360
361			if ((fd = open(filename,O_CREAT\|O_RDWR,S_IRWXU)) == -1) {
362			perror("memzone_create_file: open");
363			return(-1);
364			}
365
366			if (ftruncate(fd,len) == -1) {
367			perror("memzone_create_file: ftruncate");
368			close(fd);
369			return(-1);
370			}
371
372			*ptr = memzone_map_file(fd,len);
373
374			if (!*ptr) {
375			close(fd);
376			fd = -1;
377			}
378
379			return(fd);
380			}
381
382			/* Open a file to serve as a COW memory zone */
383			int memzone_open_cow_file(char filename,size_t len,u_char *ptr)
384			{
385			int fd;
386
387			if ((fd = open(filename,O_RDWR,S_IRWXU)) == -1) {
388			perror("memzone_open_file: open");
389			return(-1);
390			}
391
392			*ptr = memzone_map_cow_file(fd,len);
393
394			if (!*ptr) {
395			close(fd);
396			fd = -1;
397			}
398
399			return(fd);
400			}
401
402			/* Open a file and map it in memory */
403			int memzone_open_file(char filename,u_char ptr,off_t fsize)
404			{
405			struct stat fprop;
406			int fd;
407
408			if ((fd = open(filename,O_RDWR,S_IRWXU)) == -1)
409			return(-1);
410
411			if (fstat(fd,&fprop) == -1)
412			goto err_fstat;
413
414			*fsize = fprop.st_size;
415			if (!(ptr = memzone_map_file(fd,fsize)))
416			goto err_mmap;
417
418			return(fd);
419
420			err_mmap:
421			err_fstat:
422			close(fd);
423			return(-1);
424			}
425
426			/* Compute NVRAM checksum */
427			m_uint16_t nvram_cksum(m_uint16_t *ptr,size_t count)
428			{
429			m_uint32_t sum = 0;
430
431			while(count > 1) {
432			sum = sum + ntohs(*ptr);
433			ptr++;
434			count -= sizeof(m_uint16_t);
435			}
436
437			if (count > 0)
438			sum = sum + ((ntohs(*ptr) & 0xFF) << 8);
439
440			while(sum>>16)
441			sum = (sum & 0xffff) + (sum >> 16);
442
443			return(~sum);
444			}
445	dpavlin	7
446			/* Byte-swap a memory block */
447			void mem_bswap32(void *ptr,size_t len)
448			{
449			m_uint32_t *p = ptr;
450			size_t count = len >> 2;
451			int i;
452
453			for(i=0;i<count;i++,p++)
454			p = swap32(p);
455			}
456	dpavlin	8
457			/* Reverse a byte */
458			m_uint8_t m_reverse_u8(m_uint8_t val)
459			{
460			m_uint8_t res = 0;
461			int i;
462
463			for(i=0;i<8;i++)
464			if (val & (1 << i))
465			res \|= 1 << (7 - i);
466
467			return(res);
468			}