socan_util.c

 
/* socan_util.c: Miscellaneous functions for test programs.
*/
 
#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <ctype.h>
#include <stdarg.h>
#include <stdbool.h>
 
/* nanosleep: */
#include <time.h>
 
#include <socan.h>
 
#include <socan_util.h>
 
/* ----------------------------------------------
 * option parsing
 * ---------------------------------------------- */
 
int match(int argc, char *argv[], int *index, option *opts, int opt_no)
  /* returns match index or -1 */
  {
    int i;
    char *st;
 
    if (*index >= argc)
      return -1;
    st= argv[*index];
    for(i=0; i<opt_no; i++)
      {
        if (0==strcmp(opts[i].opt, st))
          {
            (*index)++;
            return opts[i].index;
          }
      }
    return -1;
  }
 
int match_int(int argc, char *argv[], int *index, int *num)
  {
    int i;
 
    if (*index >= argc)
      return -1;
    i= sscanf(argv[*index], "%d", num);
    if (i==1)
      {
        (*index)++;
        return 0; /* ok */
      }
    return -2;  /* not a number */
  }
 
int match_float(int argc, char *argv[], int *index, double *num)
  {
    int i;
 
    if (*index >= argc)
      return -1;
    i= sscanf(argv[*index], "%lf", num);
    if (i==1)
      {
        (*index)++;
        return 0; /* ok */
      }
    return -2;  /* not a number */
  }
 
int match_st(int argc, char *argv[], int *index, char **st)
  {
    if (*index >= argc)
      return -1;
    *st= argv[*index];
    (*index)++;
    return 0; /* ok */
  }
 
void my_exit(char *format, ...)
  {
    va_list ap;
 
    va_start(ap, format);
    vfprintf(stderr, format, ap);
    va_end(ap);
    exit(1);
  }
 
/* ----------------------------------------------
 * quiet level setting
 * ---------------------------------------------- */
 
void eval_quietlevel(int quietlevel, int *scale, char *ch, int *l)
  {
    *scale= 0;
    *ch= 0;
    *l= 0;
    if (quietlevel<=0)
      return;
    if (quietlevel>5)
      quietlevel= 5;
    switch(quietlevel)
      { 
        case 5: *scale=10000;
                *ch='Z';
                break;
        case 4: *scale=1000;
                *ch='T';
                break;
        case 3: *scale=100;
                *ch='O';
                break;
        case 2: *scale=10;
                *ch='o';
                break;
        case 1: *scale=1;
                *ch='.';
                break;
      };
    if (*scale!=0)
      printf("one '%c' are %d loops!\n",*ch,*scale);
  }
 
/* ----------------------------------------------
 * dump function
 * ---------------------------------------------- */
 
void nice_dump(void *ptr, int len, int addr)
  {
    unsigned char *p= (unsigned char *)ptr;
    unsigned char *q;
    int  lcount;
 
    while (len>0)
      { printf("%8X: ", addr);
        for(q=p, lcount=0; (lcount<16) && (lcount<len); lcount++, q++)
          printf("%2X ",*q);
        for( ;lcount<16; lcount++)
          printf("   ");
        printf("  \"");
        for(q=p, lcount=0; (lcount<16) && (lcount<len); lcount++, q++)
          if (isprint(*q))
            printf("%c",*q);
          else
            printf(" ");
        printf("\"\n");
        p+= lcount;
        addr+= lcount;
        len-= lcount;
      };
  }
 
/* ----------------------------------------------
 * time module
 * ---------------------------------------------- */
 
void mldelay(long milliseconds)
/* millisecond-delay, works without "dirty tricks"
   warning: the actual resolution may be smaller, e.g 1/60th of a second*/
  {
    struct timespec rq,rt;
 
    /* POSIX-conform wait */
    if (milliseconds >= 1000L)
      { rq.tv_sec = milliseconds / 1000L;
        rq.tv_nsec= (milliseconds-1000L*rq.tv_sec) * 1000000L;
      }
    else
      { rq.tv_sec = 0;
        rq.tv_nsec= milliseconds * 1000000L;
      };
    nanosleep( &rq, &rt);
  }
 
void micdelay(unsigned long microseconds)
  { struct timespec rq,rt;
 
    if (microseconds >= 1000000)
      { rq.tv_sec = microseconds / 1000000;
        rq.tv_nsec= (microseconds-1000000*rq.tv_sec) * 1000;
      }
    else
      { rq.tv_sec = 0;
        rq.tv_nsec= microseconds * 1000;
      };
    nanosleep( &rq, &rt);
  }
 
unsigned long timestamp_ms(void)
  {
    struct timespec rq;
 
    clock_gettime(CLOCK_REALTIME, &rq);
    return rq.tv_sec *1000 + rq.tv_nsec / 1000000L;
  }
 
unsigned long timestamp_us(void)
  {
    struct timespec rq;
 
    clock_gettime(CLOCK_REALTIME, &rq);
    return rq.tv_sec *1000000 + rq.tv_nsec / 1000L;
  }
 
/* ----------------------------------------------
 * init socan
 * ---------------------------------------------- */
 
bool init_socan(char **interfaces,
                int interface_no,
                int tracelevel,
                int errprintlevel,
                int reader_prio,
                int writer_prio)
/* if reader_prio and writer_prio >=0, use real time priorities */
  {
    int i;
    bool use_rt_prio= false;
 
    if ((reader_prio >=0) && (writer_prio >=0))
      use_rt_prio= true;
 
    if (!socan_realtime_setup(use_rt_prio,
                              false, /* require_rt_priority */
                              reader_prio,
                              writer_prio))
      {
        printf("%s line %d: socan_realtime_setup failed\n",
               __FILE__, __LINE__);
        return false;
      }
 
    for(i=0; i<interface_no; i++)
      {
        int p= socan_add_port(interfaces[i]);
        if (p<0)
          {
            printf("%s line %d: socan_add_port failed\n",
                   __FILE__, __LINE__);
            return false;
          }
      }
    if (!socan_init())
      {
        printf("%s line %d: socan_init failed\n",
               __FILE__, __LINE__);
        return false;
      }
    if (errprintlevel>=0)
      printf("set errprintlevel to %d\n",
             socan_errprintlevel(errprintlevel));
    if (tracelevel>=0)
      printf("set tracelevel to %d\n",
             socan_tracelevel(tracelevel));
    return true;
  }