socan.c

Go to the documentation of this file.

 
#include <assert.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <unistd.h>
#include <stdarg.h>
 
#include <stdbool.h>
 
#include <errno.h>
 
#include <net/if.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
 
/* for gettimeofday: */
#include <sys/time.h>
 
/* nanosleep: */
#include <time.h>
 
/* header for semaphores: */
#include <semaphore.h>
 
/* __USE_GNU must be defined in order to use pthread_setname_np and
 * pthread_getname_np: */
#define __USE_GNU
/* threads, mutexes: */
#include <pthread.h>
 
/* message queues */
#include <mqueue.h>
 
#include <linux/net_tstamp.h>
#include <linux/sockios.h>
 
#include <linux/can.h>
#include <linux/can/raw.h>
 
/*
 * virtual CAN interface, see:
 * https://www.pragmaticlinux.com/2021/10/how-to-create-a-virtual-can-interface-on-linux/
 
 
   Load the vcan kernel module:
     $ sudo modprobe vcan
   Create the virtual CAN interface:
     $ sudo ip link add dev vcan0 type vcan
   Bring the virtual CAN interface online:
     $ sudo ip link set up vcan0
 
   Afterwards, you can run the
     $ ip addr | grep "can"
   command to verify that the virtual CAN interface is available and online on your Linux system:
 
 * couple two virtual interfaces:
 * https://stackoverflow.com/questions/54296852/how-to-connect-two-vcan-ports-in-linux
 
   sudo modprobe can-gw
   sudo cangw -A -s vcan0 -d vcan1 -e
   sudo cangw -A -s vcan1 -d vcan0 -e
 
   test this program with:
     terminal 1:
       $ ./socantest
     terminal 2:
       $ cansend vcan0 '190#DEADBEEF'
 */
 
/* PEAK PCAN-PCI Express FD 
 
make and install driver
 
    $ sudo modprobe pcan
 
    $ sudo ip link set can0 type can bitrate 1000000
    $ sudo ip link set up can0
    $ sudo ip link set can0 txqueuelen 1000
    $ sudo ip link set can1 type can bitrate 1000000
    $ sudo ip link set up can1
    $ sudo ip link set can1 txqueuelen 1000
 
*/
 
#include <socan.h>
 
/* ----------------------------------------------
 * constants
 * ---------------------------------------------- */
 
#define READER_DEFAULT_PRIO 97
 
#define WRITER_DEFAULT_PRIO 97
 
#define MAX_WQ_MESSAGES 32
 
#define MAX_QR_MESSAGES 32
 
/* ----------------------------------------------
 * types
 * ---------------------------------------------- */
 
typedef enum
  { 
    UNINITIALIZED= 0,    
    ERROR= 1,            
    INITIALIZED= 2       
  } socan_module_state;
 
typedef struct sq_elm
  { 
    unsigned short cob;  
    unsigned short port; 
  } sq_elm;
 
typedef struct
  {
    int elements;            
    int head, tail;          
    sq_elm *data;            
    pthread_mutex_t qmutex;  
    sem_t qsem;              
    bool empty;              
  } squeue;
 
struct socan_obj_s
  {
    unsigned char port;
    unsigned short cob;
    unsigned char length;
    unsigned char received_length;
    unsigned char buffer[SOCAN_FRAME_LENGTH];
    unsigned char buffer2[SOCAN_FRAME_LENGTH]; /* only for RTR */
    bool use_buffer2;                          /* only for RTR */
    bool use_queue;                            /* for queue read */
    socan_obj_type type;
    struct timeval tv;
    struct timeval tv_request;
    unsigned int tmo_ms;
    unsigned long tm_inhibit_us;    /* inhibit time in microseconds */
    unsigned short urcnt;  /* Made equal to ircnt at the end of all read
                              functions. So urcnt==ircnt means no new
                              unread data is in the CAN object buffer. */
    unsigned short ircnt;  /* Incremented each time the reader task receives
                              data for a CAN object. */
    unsigned char w_echo;  /* set to 0 when something should be written to 
                              the CAN bus, set to 1 when the echo was received. */
    unsigned char  ulock;  /* 1 if the user task is currently changing the
                              structure */
    void *user_ptr;
    struct socan_hdl_s *hdl_s;
  };
 
typedef struct
  {
    struct socan_obj_s *objects[COB_NO];
  } cob_dict;
 
struct socan_hdl_s
  {
    sem_t thread_sem;
    pthread_mutex_t mutex;
    bool is_active;
    squeue qread_queue;
  };
 
typedef struct
  {
    char *interface_name;
    int interface_index;
    int interface_index_valid;
    unsigned char port; /* port number, starts with 0 */
    unsigned int bitrate; /* Bitrate in bit/s, initially 0.
                             Used to buffer the bitrate, if requested with
                             socan_interface_bitrate */
    bool is_virtual;      /* true for virtual CAN bus interfaces like
                             "vcan0", "vcan1" ... */
    squeue writer_squeue;
    cob_dict *dict;
  } can_port;
 
/* ----------------------------------------------
 * variables
 * ---------------------------------------------- */
 
static socan_module_state socan_state= UNINITIALIZED;
 
static bool socan_use_rt_priority= true;
static bool socan_require_rt_priority= false;
 
static int socan_reader_priority= READER_DEFAULT_PRIO;
static int socan_writer_priority= WRITER_DEFAULT_PRIO;
 
static pthread_t socan_read_threads[SOCAN_MAX_PORTS];
static pthread_t socan_write_threads[SOCAN_MAX_PORTS];
 
static pthread_mutex_t global_semaphore;
 
static can_port can_ports[SOCAN_MAX_PORTS];
static unsigned char can_ports_no= 0;
 
static int thread_count=0;
 
/* ----------------------------------------------
 * errprint module
 * ---------------------------------------------- */
 
static int errprintlevel= 1;
/* 0: no output
 * 1: output of severe errors, system calls that failed
 * 2: output of usage errors, invalid parameters
 * 3: output of run-time errors (e.g. congestion)
 */
 
static void errprint(int level, const char *function, const int line,
                     char *format, ...)
  {
    va_list args;
    char mypre[80];
    char mymsg[240];
 
    if (level>errprintlevel)
      return;
    va_start(args, format);
    snprintf(mypre, 80, "socan error in %s (line %d): ", function, line);
    vsnprintf(mymsg, 240, format, args);
    fprintf(stderr, "%s%s", mypre, mymsg);
    va_end(args);
  }
 
#define ERRPRINT(lv, ...) errprint(lv, __func__, __LINE__, __VA_ARGS__)
 
#define ERRPRINT_FUNC(lv, FUNC, ...) errprint(1, FUNC, __LINE__, __VA_ARGS__)
 
#define ERRPRINT_ERRNO(syscall) \
  errprint(1, __func__, __LINE__, \
           syscall " failed, errno %d (%s)\n", errno, strerror(errno))
 
#define ERRPRINT_RC(syscall, rc) \
  errprint(1, __func__, __LINE__, \
           syscall " failed, error %d (%s)\n", rc, strerror(rc))
 
#define ERRPRINT_RC_FUNC(FUNC, syscall, rc) \
  errprint(1, FUNC, __LINE__, \
           syscall " failed, error %d (%s)\n", rc, strerror(rc))
 
int socan_errprintlevel(int level)
  /* get/set errprintlevel,
   * level>=0: set this level
   * level<0 : do not change level
   * returns: the current or changed errprintlevel
   */
  {
    if (level>=0)
      errprintlevel= level;
    return errprintlevel;
  }
 
/* ----------------------------------------------
 * trace module
 * ---------------------------------------------- */
 
static int tracelevel= 0;
/* 0: no traces
 * 1: traces in user functions
 * 2: traces in reader/writer
 */
 
static void trace(int level, char *format, ...)
  {
    va_list args;
 
    if (level>tracelevel)
      return;
    va_start(args, format);
    vprintf(format, args);
    va_end(args);
  }
 
static void trace_object(int level,
                         const char *TASK,
                         const struct socan_obj_s *obj, 
                         unsigned char port,
                         unsigned short cob,
                         const struct can_frame *frame_)
  {
    char databuf[30];
 
    if (level>tracelevel)
      return;
 
    if (frame_->can_dlc > SOCAN_FRAME_LENGTH)
      {
        snprintf(databuf, 30, "CAN-LENGTH-OVERFLOW");
      }
    else
      {
        int i;
        char *p= databuf;
        for (i = 0; i < frame_->can_dlc; i++)
          {
            if (i!=0)
              *(p++)= ' ';
            sprintf(p, "%02X", frame_->data[i]);
            p+= 2;
          }
        *p= 0;
      }
    printf("(%s) port:%d t:%ld.%06ld  cob:0x%03X data: %s\n",
            TASK, port, obj->tv.tv_sec, obj->tv.tv_usec, cob, databuf);
  }
 
int socan_tracelevel(int level)
  /* get/set tracelevel,
   * level>=0: set this level
   * level<0 : do not change level
   * returns: the current or changed tracelevel
   */
  {
    if (level>=0)
      tracelevel= level;
    return tracelevel;
  }
 
/* ----------------------------------------------
 * socan_rc
 * ---------------------------------------------- */
 
#define CCODE(x) if (rc==x) return(#x)
/*@EX*/
char *socan_str_rc(socan_rc rc)
  { 
    CCODE(SOCAN_OK);
    CCODE(SOCAN_TIMEOUT);
    CCODE(SOCAN_OLD);
    CCODE(SOCAN_LOST);
    CCODE(SOCAN_WAIT);
    CCODE(SOCAN_EXISTS);
    CCODE(SOCAN_LENGTH_ERR);
    CCODE(SOCAN_PORT_ERR);
    CCODE(SOCAN_COB_ERR);
    CCODE(SOCAN_NOT_RO_ERR);
    CCODE(SOCAN_NOT_WO_ERR);
    CCODE(SOCAN_TYPE_ERR);
    CCODE(SOCAN_NOT_EXISTS_ERR);
    CCODE(SOCAN_NOT_OWNED_ERR);
    CCODE(SOCAN_EXISTS_ERR);
    CCODE(SOCAN_ERROR);
    ERRPRINT(1, "unexpected return code: %d\n", rc);
    return("UNKNOWN ERROR CODE!");
  }
#undef CCODE
 
bool socan_rc_is_error(socan_rc rc)
  {
    switch (rc)
      {
        case SOCAN_OK:
          return false;
        case SOCAN_TIMEOUT:
          return false;
        case SOCAN_OLD:
          return false;
        case SOCAN_LOST:
          return false;
        case SOCAN_WAIT:
          return false;
        case SOCAN_EXISTS:
          return false;
        case SOCAN_LENGTH_ERR:
          return true;
        case SOCAN_PORT_ERR:
          return true;
        case SOCAN_COB_ERR:
          return true;
        case SOCAN_NOT_RO_ERR:
          return true;
        case SOCAN_NOT_WO_ERR:
          return true;
        case SOCAN_TYPE_ERR:
          return true;
        case SOCAN_NOT_EXISTS_ERR:
          return true;
        case SOCAN_NOT_OWNED_ERR:
          return true;
        case SOCAN_EXISTS_ERR:
          return true;
        case SOCAN_ERROR:
          return true;
        default:
          ERRPRINT(1, "unexpected return code: %d\n", rc);
          return true;
      }
  }
 
 
/* ----------------------------------------------
 * check module
 * ---------------------------------------------- */
 
/* forward declaration: */
static struct socan_obj_s *cob_dict_lookup(cob_dict *dict, unsigned short cob);
 
static bool check_len(unsigned char len, const char *func)
  {
    if (len>SOCAN_FRAME_LENGTH)
      {
        ERRPRINT_FUNC(2, func, "len %d invalid", len);
        return false;
      }
    return true;
  }
 
static bool check_cob(unsigned short cob, const char *func)
  {
    if (cob>=COB_NO)
      {
        ERRPRINT_FUNC(2, func, "cob %d invalid", cob);
        return false;
      }
    return true;
  }
 
static socan_rc check_get_cob(unsigned short cob, const char *func,
                              can_port *cp,
                              struct socan_obj_s **obj,
                              bool in_user_task)
  {
    if (cob>=COB_NO)
      {
        ERRPRINT_FUNC(2, func, "cob %d invalid", cob);
        return SOCAN_COB_ERR;
      }
    *obj= cob_dict_lookup(cp->dict, cob);
    if (*obj == NULL)
      {
        if (in_user_task)
          {
            /* note: it would be better to have the port number in this error
             * message */
            ERRPRINT_FUNC(2, func, "can object for cob %d is not defined", cob);
          }
        return SOCAN_NOT_EXISTS_ERR;
      }
    if ((*obj)->hdl_s == NULL)
      {
        if (in_user_task)
          {
            /* note: it would be better to have the port number in this error
             * message */
            ERRPRINT_FUNC(2, func, "can object for cob %d has no owner", cob);
          }
        return SOCAN_NOT_OWNED_ERR;
      }
    return SOCAN_OK;
  }
 
static bool check_get_port(unsigned char port, const char *func, can_port **cp)
  {
    if (port>=can_ports_no)
      {
        ERRPRINT_FUNC(2, func, "port %d invalid", port);
        return false;
      }
    *cp= &(can_ports[port]);
    return true;
  }
 
/* ----------------------------------------------
 * time module
 * ---------------------------------------------- */
 
static void tv_to_str(const struct timeval *tv, char *buf, int buflen)
  {
    time_t nowtime;
    struct tm nowtm;
    int len;
 
    nowtime = tv->tv_sec;
    localtime_r(&nowtime, &nowtm);
    len= strftime(buf, buflen, "%Y-%m-%d %H:%M:%S", &nowtm);
    snprintf(buf+len, buflen-len, ".%06ld", tv->tv_usec);
  }
 
static void us_to_tv(unsigned long tv_us, struct timeval *tv)
  {
    ldiv_t d= ldiv(tv_us, 1000000);
    tv->tv_sec= d.quot;
    tv->tv_usec= d.rem;
  }
 
static unsigned long tv_to_us(const struct timeval *tv)
  { /* convert timeval to microseconds */
    return tv->tv_sec * 1000000 + tv->tv_usec;
  }
 
static unsigned long tv_to_ms(const struct timeval *tv)
  { /* convert timeval to microseconds */
    return tv->tv_sec * 1000 + tv->tv_usec / 1000;
  }
 
static unsigned long tm_ms_now(void)
  /* calculate an absolute timeout in a timespec structure from
   * a relative timeout given in milliseconds. 
   */
  {
    struct timeval now;
    gettimeofday(&now, NULL);
    return tv_to_ms(&now);
  }
 
static unsigned long tm_us_now(void)
  /* get current time in microseconds */
  {
    struct timeval now;
    gettimeofday(&now, NULL);
    return tv_to_us(&now);
  }
 
static unsigned long tm_ms_diff_to_now(const struct timeval *tv)
  /* returns now - tv1 in milliseconds */
  {
    return tm_ms_now() - tv_to_ms(tv);
  }
 
static void tv_now(struct timeval *tv)
  {
    gettimeofday(tv, NULL);
  }
 
static void calc_abs_timeout(int timeout_ms, struct timespec *ts)
  /* calculate an absolute timeout in a timespec structure from
   * a relative timeout given in milliseconds. 
   */
  {
    int s =0;
    int ms= timeout_ms;
 
    if (ms >= 1000)
      {
        s= ms / 1000;
        ms-= s * 1000;
      }
 
    struct timeval now;
    gettimeofday(&now, NULL);
    ts->tv_sec= now.tv_sec + s;
    ts->tv_nsec= now.tv_usec * 1000 + ms * 1000000;
    if (ts->tv_nsec >= 1000000000)
      {
        ts->tv_nsec-= 1000000000;
        ts->tv_sec++;
      }
  }
 
static 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);
  }
 
/* ----------------------------------------------
 * thread module
 * ---------------------------------------------- */
 
/* max. thread name length according to pthread_setname_np man page: */
#define THREAD_NAMELEN 16
 
static char *thread_suffix(bool is_reader, int port)
  {
    static char buffer[THREAD_NAMELEN];
 
    if (is_reader)
      snprintf(buffer, THREAD_NAMELEN, "-r%d", port);
    else
      snprintf(buffer, THREAD_NAMELEN, "-w%d", port);
    return buffer;
  }
 
 
static bool get_curr_prio(int *policy, int *min_prio, int *max_prio,
                          int *curr_prio)
  /* Note: usually this returns:
   * *policy= SCHED_OTHER
   * *min_prio= 0
   * *max_prio= 0
   * *curr_prio= 0
   */
  {
    int rc;
    struct sched_param param;
    rc= pthread_getschedparam(pthread_self(), policy, &param);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_getschedparam", rc);
        return false;
      }
    *max_prio= sched_get_priority_max(*policy);
    *min_prio= sched_get_priority_min(*policy);
    *curr_prio= param.sched_priority;
    return true;
  }
 
static bool init_attr_p(pthread_attr_t *attr_p, int stack, int prio)
  /* prio<0: take standard priority and scheduling policy */
  /* Setting the policy to SCHED_FIFO is only allowed for user "root". If the
     application has not the permissions to to this, the thread is created with
     regular priority.
 
     To have real time priorities you should instead start your CAN bus
     application with:
       $ sudo PROGRAM
   */
  {
    int rc;
    int curr_policy, min_prio, max_prio, curr_prio;
    struct sched_param s_param;
 
    rc= pthread_attr_init(attr_p);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_attr_init", rc);
        return false;
      }
    /* On debian 12 (bullseye) PTHREAD_STACK_MIN doesn't seem to be defined: */
#ifndef PTHREAD_STACK_MIN
#define PTHREAD_STACK_MIN 16384
#endif
    rc= pthread_attr_setstacksize(attr_p,
              (stack < PTHREAD_STACK_MIN) ? PTHREAD_STACK_MIN : stack);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_attr_setstacksize", rc);
        return false;
      }
    /* Get scheduling policy, minimum and maximum priority for this policy and
     * the current priority of the parent (this) thread: */
    if (!get_curr_prio(&curr_policy, &min_prio, &max_prio, &curr_prio))
      return false;
 
    if (prio<0)
      { /* standard policy and priority */
        rc= pthread_attr_setschedpolicy(attr_p, curr_policy);
        if (rc!=0)
          {
            ERRPRINT_RC("pthread_attr_setschedpolicy", rc);
            return false;
          }
        rc = pthread_attr_setinheritsched(attr_p, PTHREAD_INHERIT_SCHED);
        if (rc!=0)
          {
            ERRPRINT_RC("pthread_attr_setinheritsched", rc);
            return false;
          }
        s_param.sched_priority= curr_prio;
        rc= pthread_attr_setschedparam(attr_p, &s_param);
        if (rc!=0)
          {
            ERRPRINT_RC("pthread_attr_setschedparam", rc);
            return false;
          }
      }
    else
      { /* read-time policy and priority */
        rc= pthread_attr_setschedpolicy(attr_p, SCHED_FIFO);
        if (rc!=0)
          {
            ERRPRINT_RC("pthread_attr_setschedpolicy", rc);
            return false;
          }
        /* we must set PTHREAD_EXPLICIT_SCHED, PTHREAD_INHERIT_SCHED is
         * the default, which means as a default new threads inherit the
         * scheduling policy AND priority from their parents regardless of the
         * settings in attr_p. */
        rc = pthread_attr_setinheritsched(attr_p, PTHREAD_EXPLICIT_SCHED);
        if (rc!=0)
          {
            ERRPRINT_RC("pthread_attr_setinheritsched", rc);
            return false;
          }
        s_param.sched_priority= prio;
        rc= pthread_attr_setschedparam(attr_p, &s_param);
        if (rc!=0)
          {
            ERRPRINT_RC("pthread_attr_setschedparam", rc);
            return false;
          }
      }
    return true;
  }
 
static bool thread_start(void *(*th)(void*), pthread_t *pst,
                         char *suffix,
                         int stack,
                         int prio,
                         void *arg)
/* stack may be 0 or prio may be -1 in order to use default-values */
  { 
    int rc;
    pthread_attr_t attr;
    char thread_name[THREAD_NAMELEN];
 
    if (suffix != NULL)
      {
        int l, m;
 
        /* Get name of parent thread: */
        rc = pthread_getname_np(pthread_self(), thread_name, THREAD_NAMELEN);
        if (rc!=0)
          { 
            ERRPRINT_RC("pthread_getname_np", rc);
            return false;
          }
        /* combine parent thread name and suffix: */
        l= strlen(thread_name);
        m= strlen(suffix);
        if (m>THREAD_NAMELEN-1)
          m= THREAD_NAMELEN-1;
        if (l+m > THREAD_NAMELEN-1)
          l= THREAD_NAMELEN-1 - m;
        strncpy(thread_name+l, suffix, m);
        thread_name[THREAD_NAMELEN-1]= 0;
      }
 
    if (!socan_use_rt_priority)
      prio= -1;
 
    if (!init_attr_p(&attr, stack, prio))
      {
        ERRPRINT(1, "init_attr");
        return false;
      }
 
    rc = pthread_create( pst, &attr, th, arg);
    if ((rc==EPERM) && (prio>=0)) /* operation not permitted */
      {
        if (socan_require_rt_priority)
          {
            ERRPRINT(1, "Error: creating thread %s with policy\n"
                    "\tSCHED_FIFO and priority %d failed.\n",
                    thread_name, prio);
            return false;
          }
        ERRPRINT(1, "Warning: creating thread with policy\n"
                    "\tSCHED_FIFO and priority %d failed.\n"
                    "\tUsing SCHED_OTHER and priority 0 from now on.\n",
                    prio);
        /* do not try again to use real time priorities */
        socan_use_rt_priority= false;
 
        /* try with standard priority */
        if (!init_attr_p(&attr, stack, -1))
          return false;
        /* now try again */
        rc = pthread_create( pst, &attr, th, arg);
      }
    if (rc!=0)
      { 
        ERRPRINT_RC("pthread_create", rc);
        return false;
      }
    if (suffix!=NULL)
      {
        rc = pthread_setname_np(*pst, thread_name);
        if (rc!=0)
          { 
            ERRPRINT_RC("pthread_setname_np", rc);
            return false;
          }
      }
    return true;
  }
 
/* ----------------------------------------------
 * mutex module
 * ---------------------------------------------- */
 
static bool mutex_init(pthread_mutex_t *mutex)
  /* initialize mutex with PTHREAD_PRIO_INHERIT */
  { 
    int rc;
 
    pthread_mutexattr_t mutexattr;
 
    rc= pthread_mutexattr_init(&mutexattr);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_mutexattr_init", rc);
        return false;
      }
    rc= pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_mutexattr_setprotocol", rc);
        return false;
      }
    rc= pthread_mutex_init(mutex, &mutexattr);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_mutex_init", rc);
        return false;
      }
    return true;
  }
 
static bool mutex_delete(pthread_mutex_t *mutex)
  {
    int rc;
 
    rc= pthread_mutex_destroy(mutex);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_mutex_destroy", rc);
        return false;
      }
    return true;
  }
 
static bool mutex_lock(pthread_mutex_t *mutex, const char *func)
  {
    int rc= pthread_mutex_lock(mutex);
    if (!rc)
      return true;
    ERRPRINT_RC_FUNC(func, "pthread_mutex_lock", rc);
    return false;
  }
 
static bool mutex_unlock(pthread_mutex_t *mutex, const char *func)
  {
    int rc= pthread_mutex_unlock(mutex);
    if (!rc)
      return true;
    ERRPRINT_RC_FUNC(func, "pthread_mutex_unlock", rc);
    return false;
  }
 
#define MUTEX_LOCK(mutex) mutex_lock(mutex, __func__)
 
#define MUTEX_UNLOCK(mutex) mutex_unlock(mutex, __func__)
 
/* ----------------------------------------------
 * message queue module
 * ---------------------------------------------- */
 
/*
       x               1 elm
    0  1  2  3  4  5
       H  T   
 
       x  x  x         3 elm
    0  1  2  3  4  5
       H        T
 
    x           x  x   3 elm
    0  1  2  3  4  5
       T        H
 
    x  x  x  x  x  x   5 elm, full !!
    0  1  2  3  4  5
          HT     
 
                    
    0  1  2  3  4  5   empty, empty flag set
       HT        
*/
 
static bool squeue_init(squeue *q, unsigned int elements)
  {
    q->data= NULL;
    q->elements= 0;
    q->head= 0;
    q->tail= 0;
 
    if (!mutex_init(&(q->qmutex)))
      {
        ERRPRINT(1, "mutex_init failed\n");
        return false;
      }
    if (sem_init(&(q->qsem), 0, 0)) /* initially taken */
      {
        ERRPRINT_ERRNO("sem_init");
        mutex_delete(&(q->qmutex));
        return false;
      }
 
    q->data= calloc(elements, sizeof(sq_elm));
    q->elements= elements;
    q->empty= true;
    return true;
  }
 
static int squeue_inc(const squeue *q, int index)
  { /* increment index */
    index++;
    if (index >= q->elements)
      index= 0;
    return index;
  }
 
#if 0
static bool squeue_delete(squeue *q)
  {
    free(q->data);
    if (!mutex_delete(&(q->qmutex)))
      {
        ERRPRINT(1, "mutex_delete failed\n");
        return false;
      }
    if (sem_destroy(&(q->qsem)))
      {
        ERRPRINT_ERRNO("sem_destroy");
        return false;
      }
    return true;
  }
#endif
 
static bool squeue_add(squeue *q, unsigned short cob, unsigned char port)
  /* add element to squeue (at "tail") */
  {
    sq_elm elm;
 
    elm.cob= cob;
    elm.port= port;
 
    MUTEX_LOCK(&(q->qmutex));
 
    if ((q->tail == q->head) && (!q->empty))
      { /* squeue is actually full */
        MUTEX_UNLOCK(&(q->qmutex));
        return false;
      }
 
    /* squeue is currently empty */
    q->data[q->tail]= elm;
    q->tail= squeue_inc(q, q->tail);
    q->empty= false;
    if (sem_post(&(q->qsem)))
      ERRPRINT_ERRNO("sem_post");
    MUTEX_UNLOCK(&(q->qmutex));
    return true;
  }
 
static void squeue_get(squeue *q, unsigned short *cob, unsigned char *port)
  /* remove element from squeue (at "head") */
  /* this is called in a real-time task, it may interrupt 
   * squeue_add at *any* time 
   * blocks until qmutex is given
   */
  {
    sq_elm *elm;
 
    for(;;)
      {
        if (sem_wait(&(q->qsem)))
          {
            ERRPRINT_ERRNO("sem_wait");
            continue;
          }
        MUTEX_LOCK(&(q->qmutex));
        if (!q->empty)
          { /* squeue is not empty */
            break;
          }
        MUTEX_UNLOCK(&(q->qmutex));
      }
    elm= &(q->data[q->head]);
    *cob= elm->cob;
    *port= (unsigned char)(elm->port);
    q->head= squeue_inc(q, q->head);
    if (q->head == q->tail)
      q->empty= true;
    MUTEX_UNLOCK(&(q->qmutex));
  }
 
/* ----------------------------------------------
 * "ip" utility calls
 * ---------------------------------------------- */
 
static bool interface_exists(const char *device)
  /* returns -1 when interface doesn't exist */
  {
    FILE *cmd;
    char buf[120];
    int rc;
 
    /* check for CAN interface by calling
     * ip link show DEVICE
     */
    snprintf(buf, sizeof(buf), "ip link show %s 2>&1", device);
    cmd=popen(buf, "r");
    while (fgets(buf, sizeof(buf), cmd) !=NULL)
      {
        /* printf("^%s\n", buf); */
      }
    rc= pclose(cmd);
    /* rc: return code of command */
    return (rc==0);
  }
 
/* ----------------------------------------------
 * socan bitrate module
 * ---------------------------------------------- */
 
/* Note:
 
   typical output of ip -details link show vcan0:
 
   4: vcan0: <NOARP,UP,LOWER_UP> mtu 72 qdisc noqueue state UNKNOWN mode DEFAULT group default qlen 1000
       link/can  promiscuity 0 allmulti 0 minmtu 0 maxmtu 0 
       vcan numtxqueues 1 numrxqueues 1 gso_max_size 65536 gso_max_segs 65535 tso_max_size 65536 tso_max_segs 65535 gro_max_size 65536 gso_ipv4_max_size 65536 gro_ipv4_max_size 65536 
 
   typical output of ip -details link show can0:
 
   9: can0: <NOARP,UP,LOWER_UP> mtu 16 qdisc pfifo_fast state UP mode DEFAULT group default qlen 10
       link/can  promiscuity 0 allmulti 0 minmtu 0 maxmtu 0 
       can state ERROR-ACTIVE (berr-counter tx 0 rx 0) restart-ms 0 
      bitrate 1000000 sample-point 0.750
      tq 12 prop-seg 29 phase-seg1 30 phase-seg2 20 sjw 10 brp 1
      pcan: tseg1 1..256 tseg2 1..128 sjw 1..128 brp 1..1024 brp_inc 1
      pcan: dtseg1 1..32 dtseg2 1..16 dsjw 1..16 dbrp 1..1024 dbrp_inc 1
      clock 80000000 numtxqueues 1 numrxqueues 1 gso_max_size 65536 gso_max_segs 65535 tso_max_size 65536 tso_max_segs 65535 gro_max_size 65536 gso_ipv4_max_size 65536 gro_ipv4_max_size 65536 
   
*/
 
static bool socan_is_virtual_interface(const char *device)
  /* return true when the device name is (written here as regexp):
   * ^vcan[0-9]+$
   */
  {
    int dummy;
    char dummy2;
    int rc= sscanf(device, "vcan%d%c", &dummy, &dummy2);
    return (rc==1);
  }
 
socan_rc socan_port_bitrate(unsigned char port, unsigned int *bitrate)
  /* returns the bitrate in bits/sec.
   * returns -1 when bitrate could not be determined */
  {
    FILE *cmd;
    char buf[120];
    int rc;
    can_port *cp;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
 
    /* get CAN interface bitrate by calling
     * ip -details link show DEVICE
     */
 
    /* if DEVICE has a name like vcanNUMBER e.g. "vcan0" we assume that this is
     * a virtual CAN interface that has no bitrate. In this case we pretend
     * that it has a bitrate of 1000000 (1MBit). */
    if (cp->is_virtual)
      {
        /* return 1MBit */
        *bitrate= 1000000;
        /* cache this value: */
        cp->bitrate= *bitrate;
        return SOCAN_OK;
      }
 
    /* see if bitrate is already known: */
    if (cp->bitrate != 0)
      {
        *bitrate= cp->bitrate;
        return SOCAN_OK;
      }
 
    snprintf(buf, sizeof(buf), "ip -details link show %s",
             cp->interface_name);
    cmd=popen(buf, "r");
    while (fgets(buf, sizeof(buf), cmd) !=NULL)
      {
        /*printf("^%s\n", buf);*/
        sscanf(buf, " bitrate %ud ", bitrate);
      }
    rc= pclose(cmd);
    if (rc==0)
      {
        /* cache this value: */
        cp->bitrate= *bitrate;
        return SOCAN_OK;
      }
    /* error case here */
    *bitrate= 0;
    return SOCAN_ERROR;
  }
 
/* ----------------------------------------------
 * socan_hdl module
 * ---------------------------------------------- */
 
/* forward declaration: */
static void canobj_delete(socan_obj_h obj);
 
socan_hdl socan_open(void)
  {
    int rc;
    struct socan_hdl_s *t;
 
    if (socan_state != INITIALIZED)
      {
        ERRPRINT(1, "socan is not initialized\n");
        return NULL;
      }
    MUTEX_LOCK(&global_semaphore);
    t= calloc(1, sizeof(struct socan_hdl_s));
    if (sem_init(&(t->thread_sem), 0, 1)) /* initially not taken */
      {
        ERRPRINT_ERRNO("sem_init");
        free(t);
        MUTEX_UNLOCK(&global_semaphore);
        return NULL;
      }
    rc= pthread_mutex_init(&(t->mutex), NULL);
    if (rc!=0)
      {
        ERRPRINT_RC("pthread_mutex_init", rc);
        if (sem_destroy(&(t->thread_sem)))
          ERRPRINT_ERRNO("sem_destroy");
        free(t);
        MUTEX_UNLOCK(&global_semaphore);
        return NULL;
      }
    if (!squeue_init(&(t->qread_queue), MAX_QR_MESSAGES))
      {
        ERRPRINT(1, "squeue_init failed\n");
        if (sem_destroy(&(t->thread_sem)))
          ERRPRINT_ERRNO("sem_destroy");
        if (!mutex_delete(&(t->mutex)))
          ERRPRINT(1, "mutex_delete failed\n");
        free(t);
        MUTEX_UNLOCK(&global_semaphore);
        return NULL;
      }
    t->is_active= true;
    MUTEX_UNLOCK(&global_semaphore);
    return t;
  }
 
bool socan_close(socan_hdl h)
  {
    /* free all resources */
    unsigned char port;
    struct socan_obj_s *obj;
    unsigned short cob;
    int rc=0;
 
    h->is_active= false; /* stop reader/writer from using this */
    /* now iterate over all ports and delete all objects */
 
    MUTEX_LOCK(&global_semaphore);
    if ((rc= pthread_mutex_destroy(&(h->mutex))))
      ERRPRINT_RC("pthread_mutex_destroy", rc);
    if (sem_destroy(&(h->thread_sem)))
      {
        ERRPRINT_ERRNO("sem_destroy");
        rc=1;
      }
    for(port=0; port<can_ports_no; port++)
      {
        cob_dict *dict= can_ports[port].dict;
        for(cob=0; cob<COB_NO; cob++)
          {
            obj= cob_dict_lookup(dict, cob);
            if (obj==NULL)
              continue;
            if (obj->hdl_s != h)
              continue;
            canobj_delete(obj);
          }
      }
    free(h);
    MUTEX_UNLOCK(&global_semaphore);
    return (rc==0);
  }
 
socan_rc socan_port_device(unsigned char port, char **dev)
  /* return device name */
  {
    can_port *cp;
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    *dev= cp->interface_name;
    return SOCAN_OK;
  }
 
static void socan_hdl_give(socan_hdl h)
  { 
    int sval;
 
    if (sem_getvalue(&(h->thread_sem), &sval))
      {
        ERRPRINT_ERRNO("sem_getvalue");
        return;
      }
    if (sval>0)
      return;
    if (sem_post(&(h->thread_sem)))
      {
        ERRPRINT_ERRNO("sem_post");
        return;
      }
  }
 
static int socan_hdl_wait_tmo(socan_hdl h, struct timespec *ts)
  /* returns:
     0: ok
     1: timeout
    -1: error
   */
  { 
    int rc= sem_timedwait(&(h->thread_sem), ts);
 
    if (rc==0)
      return 0;
    if (errno==ETIMEDOUT)
      return 1;
    ERRPRINT_ERRNO("sem_timedwait");
    return -1;
  }
 
/* ----------------------------------------------
 * socan_obj module
 * ---------------------------------------------- */
 
 
static socan_obj_h canobj_new(socan_hdl hdl,
                              unsigned char port,
                              unsigned short cob,
                              unsigned char length,
                              unsigned int tmo_ms,
                              socan_obj_type type)
  {
    if (!check_len(length, __func__))
      return NULL;
    if (!check_cob(cob, __func__))
      return NULL;
    /* calloc initializes the structure with 0 bytes, so various fields are
     * initialized with 0 which is indicated by a comment here. */
    struct socan_obj_s *obj= calloc(1, sizeof(struct socan_obj_s));
    obj->port= port;
    obj->cob= cob;
    obj->length= length;
    /* received_length==0  */
    /* buffer[] filled with 0  */
    /* buffer2[] filled with 0  */
    /* use_buffer2==false  */
    /* use_queue==false  */
    obj->type= type;
    /* tv is 0 */
    /* tv_request is 0  */
    obj->tmo_ms= tmo_ms;
    /* tm_inhibit_us == 0  */
    obj->ircnt= 0;
    obj->urcnt= 0;
    obj->w_echo= 1;
    /* ulock==0 */
    obj->user_ptr= NULL;
    obj->hdl_s= hdl;
    return obj;
  }
 
static void canobj_delete(socan_obj_h obj)
  {
    void *up= obj->user_ptr;
 
    obj->user_ptr= NULL;
    if (up != NULL)
      free(up);
    free(obj);
  }
 
static unsigned char *canobj_rtr_wbuf(struct socan_obj_s *obj)
  {
    if (obj->use_buffer2)
      return obj->buffer2;
    return obj->buffer;
  }
 
static unsigned char *canobj_rtr_wbuf_other(struct socan_obj_s *obj)
  {
    if (obj->use_buffer2)
      return obj->buffer;
    return obj->buffer2;
  }
 
static void canobj_rtr_switch(struct socan_obj_s *obj)
  {
    if (obj->use_buffer2)
      obj->use_buffer2= false;
    else
      obj->use_buffer2= true;
  }
 
/* ----------------------------------------------
 * cob_dict module
 * ---------------------------------------------- */
 
 
cob_dict *cob_dict_new(void)
  {
    return calloc(1, sizeof(cob_dict));
  }
 
void cob_dict_delete(cob_dict *dict)
  {
    return free(dict);
  }
 
static struct socan_obj_s *cob_dict_lookup(cob_dict *dict, unsigned short cob)
  {
    if (!check_cob(cob, __func__))
      return NULL;
    return (dict->objects)[cob];
  }
 
static bool cob_dict_set(cob_dict *dict, unsigned short cob, void *ptr)
  /* returns false on error */
  {
    if (!check_cob(cob, __func__))
      return false;
    (dict->objects)[cob]= ptr;
    return true;
  }
 
static bool cob_dict_remove(cob_dict *dict, unsigned short cob)
  /* returns 0 on error */
  {
    if (!check_cob(cob, __func__))
      return false;
    (dict->objects)[cob]= NULL;
    return true;
  }
 
/* ----------------------------------------------
 * can_port module
 * ---------------------------------------------- */
 
int socan_add_port(const char *devicename)
  /* returns -1 on error, otherwise the port number */
  {
    can_port *cp;
    int idx;
 
    if (socan_state == INITIALIZED)
      {
        ERRPRINT(2, "cannot add ports in INITIALIZED state\n");
        return -1;
      };
    if (socan_state == ERROR)
      {
        ERRPRINT(2, "cannot add ports in ERROR state\n");
        return -1;
      };
 
    if (can_ports_no >= SOCAN_MAX_PORTS)
      {
        ERRPRINT(2, "too many ports\n");
        return -1;
      };
    if (!interface_exists(devicename))
      {
        ERRPRINT(2, "interface '%s' doesn't exist\n", devicename);
        return -1;
      };
    cp= &(can_ports[can_ports_no]);
    cp->interface_name= strdup(devicename);
    cp->interface_index_valid= 0;
    cp->interface_index= 0;
    cp->dict= cob_dict_new();
    cp->port= can_ports_no;
    cp->bitrate= 0;
    cp->is_virtual= socan_is_virtual_interface(devicename);
    if (!squeue_init(&(cp->writer_squeue), MAX_WQ_MESSAGES))
      {
        ERRPRINT(1, "squeue_init failed\n");
        return -1;
      }
    idx= can_ports_no;
    can_ports_no++;
    return idx;
  }
 
static int can_port_interface_index(can_port *cp, int sock)
  /* returns interface index, -1 on error */
  {
    int i;
 
    if (!cp->interface_index_valid)
      {
        MUTEX_LOCK(&global_semaphore);
        struct ifreq ifr;
        strcpy(ifr.ifr_name, cp->interface_name);
        ioctl(sock, SIOCGIFINDEX, &ifr);
        cp->interface_index= ifr.ifr_ifindex;
        cp->interface_index_valid= 1;
        MUTEX_UNLOCK(&global_semaphore);
      }
    i= cp->interface_index;
    return i;
  }
 
socan_rc socan_add_obj(socan_hdl hdl, unsigned char port,
                       unsigned short cob, unsigned char length,
                       unsigned int tmo_ms, socan_obj_type type)
  {
    struct socan_obj_s *obj;
    can_port *cp;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    if (!check_cob(cob, __func__))
      return SOCAN_COB_ERR;
    if (!check_len(length, __func__))
      return SOCAN_LENGTH_ERR;
    MUTEX_LOCK(&global_semaphore);
    obj= cob_dict_lookup(cp->dict, cob);
    if (obj != NULL)
      {
        if ((obj->hdl_s == hdl) && (obj->length == length) &&
            (obj->type == type) && (obj->tmo_ms == tmo_ms))
          /* can object already exists with exactly the same parameters */
          /* this is not neccesarily an error */
          {
            MUTEX_UNLOCK(&global_semaphore);
            return SOCAN_EXISTS;
          }
        if ((obj->hdl_s !=NULL) && (obj->hdl_s != hdl)) 
          /* object is not owned by *this* thread */
          {
            MUTEX_UNLOCK(&global_semaphore);
            return SOCAN_NOT_OWNED_ERR;
          }
        /* object is owned by this thread but with different parameters */
        ERRPRINT(2, "CAN object cob %d port %d already exists\n",
                 port, cob);
        MUTEX_UNLOCK(&global_semaphore);
        return SOCAN_EXISTS_ERR;
      }
    obj= canobj_new(hdl, port, cob, length, tmo_ms, type);
    cob_dict_set(cp->dict, cob, obj);
    MUTEX_UNLOCK(&global_semaphore);
    return SOCAN_OK;
  }
 
socan_rc socan_del_obj(unsigned char port, unsigned short cob)
  {
    struct socan_obj_s *obj;
    can_port *cp;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    if (!check_cob(cob, __func__))
      return SOCAN_COB_ERR;
    MUTEX_LOCK(&global_semaphore);
    obj= cob_dict_lookup(cp->dict, cob);
    /* we do not treat it as an error when the can object is not defined: */
    if (obj != NULL)
      {
        cob_dict_remove(cp->dict, cob);
        canobj_delete(obj);
      }
    MUTEX_UNLOCK(&global_semaphore);
    return SOCAN_OK;
  }
 
socan_rc socan_obj_info(socan_hdl hdl,
                        unsigned char port, unsigned short cob,
                        unsigned char *length,
                        unsigned int *tmo_ms,
                        socan_obj_type *type)
  /* object info */
  {
    struct socan_obj_s *obj;
    can_port *cp;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    if (!check_cob(cob, __func__))
        return SOCAN_COB_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, false);
    if (rc!=SOCAN_OK)
      return rc;
    if (obj->hdl_s != hdl)
      return SOCAN_NOT_OWNED_ERR;
    *length= obj->length;
    *tmo_ms= obj->tmo_ms;
    *type  = obj->type;
    return SOCAN_OK;
  }
 
socan_rc socan_obj_ts(unsigned char port, unsigned short cob, unsigned long *ts)
  /* microsecond timestamp */
  {
    struct socan_obj_s *obj;
    can_port *cp;
    unsigned long us1, us2;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    /* read timestamp again until we know we were not interrupted by the reader
     * thread: */
    do {
         us1= tv_to_us(&(obj->tv));
         us2= tv_to_us(&(obj->tv));
       } while (us1 != us2);
    *ts= us1;
    return SOCAN_OK;
  }
 
socan_rc socan_ts_to_str(unsigned long ts, char *buf, int buflen)
  {
    struct timeval tv;
    us_to_tv(ts, &tv);
    tv_to_str(&tv, buf, buflen);
    return SOCAN_OK;
  }
 
socan_rc socan_set_inhibit(unsigned char port, unsigned short cob, unsigned long inhibit_time)
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    if (obj->type != SOCAN_WRITE)
      { /* doesn't exist */
        ERRPRINT(2, "cob %d is not an ordinary write object\n", cob);
        return SOCAN_NOT_WO_ERR;
      }
    obj->tm_inhibit_us= inhibit_time;
    return SOCAN_OK;
  }
 
socan_rc socan_use_queue(unsigned char port, unsigned short cob)
  /* prepare for queue-read usage */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    if ((obj->type != SOCAN_READ) && (obj->type != SOCAN_READ_RTR))
      { /* wrong type */
        ERRPRINT(2, "cob %d is not a read or rtr-read object\n", cob);
        return SOCAN_NOT_RO_ERR;
      }
    obj->use_queue= true;
    return SOCAN_OK;
  }
 
socan_rc socan_new_user_area(unsigned char port, unsigned short cob,
                             unsigned int size,
                             void **area)
  /* allocate space for user data area */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    if (obj->user_ptr != NULL)
      free(obj->user_ptr);
    obj->user_ptr= calloc(1, size);
    *area= obj->user_ptr;
    return SOCAN_OK;
  }
 
socan_rc socan_user_area(unsigned char port, unsigned short cob, void **area)
  /* return user data area */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    *area= obj->user_ptr;
    return SOCAN_OK;
  }
 
socan_rc socan_set_user_area(unsigned char port, unsigned short cob, void *ptr)
  /* directly set the user data area */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    obj->user_ptr= ptr;
    return SOCAN_OK;
  }
 
socan_rc socan_free_user_area(unsigned char port, unsigned short cob)
  /* free user data area */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    void *up;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    up= obj->user_ptr;
    obj->user_ptr= NULL;
    if (up!=NULL)
      free(up);
    return SOCAN_OK;
  }
 
/* ----------------------------------------------
 * read and write functions
 * ---------------------------------------------- */
 
socan_rc socan_readnow(socan_hdl hdl, unsigned char port, unsigned short cob, void *data)
  /* Non blocking read :
   * This only reads the local buffer for SOCAN_READ and SOCAN_READ_RTR
   * objects. In case of SOCAN_READ_RTR it *does not* send an rtr frame.
   *
   * may return:
   * SOCAN_ERROR       : generic error
   * SOCAN_LENGTH_ERR  : data with wrong length arrived
   * SOCAN_OK          : data was read
   * SOCAN_OLD         : old data was read again
   * SOCAN_LOST        : some data was lost
   */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    int ircnt;
    unsigned char rlen;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
    if ((obj->type != SOCAN_READ) && (obj->type != SOCAN_READ_RTR))
      { /* doesn't exist */
        ERRPRINT(2, "cob %d is not a read object\n", cob);
        return SOCAN_NOT_RO_ERR;
      }
    /* read until ircnt hasn't changed, only then 
     * we know we have consistent data */
    do {
         ircnt= obj->ircnt;
         rlen= obj->received_length;
         memcpy(data, obj->buffer, obj->length);
       } while (ircnt != obj->ircnt);
    for(;;)
      {
        if (obj->urcnt == ircnt)
          {
            rc= SOCAN_OLD;
            break;
          }
        if (rlen != obj->length)
          {
            rc= SOCAN_LENGTH_ERR;
            break;
          }
        if ((unsigned short)(ircnt - obj->urcnt) > 1)
          {
            rc= SOCAN_LOST;
            break;
          }
        rc= SOCAN_OK;
        break;
      }
    obj->urcnt= ircnt;
    return rc;
  }
 
socan_rc socan_read(socan_hdl hdl, unsigned char port, unsigned short cob, void *data)
  {
  /* Blocking read with timeout for SOCAN_READ and SOCAN_READ_RTR objects. For
   * SOCAN_READ_RTR objects, it sends an RTR frame.
   *
   * may return:
   * SOCAN_ERROR       : generic error
   * SOCAN_LENGTH_ERR  : data with wrong length arrived
   * SOCAN_OK          : data was read
   * SOCAN_OLD         : old data was read again (shouldn't happen)
   * SOCAN_TIMEOUT     : timeout, no data read
   * SOCAN_LOST        : some data was lost
   */
    can_port *cp;
    struct socan_obj_s *obj;
    struct timespec ts;
    int ircnt, rc_i;
    unsigned char rlen;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
 
    switch(obj->type)
      {
        case SOCAN_READ_RTR:
          obj->urcnt= obj->ircnt;
          calc_abs_timeout(obj->tmo_ms, &ts);
          if (!squeue_add(&(cp->writer_squeue), cob, port))
            {
              ERRPRINT(3, "squeue_add failed, queue full "
                          "(size %d) port:%d cob:%d\n",
                       cp->writer_squeue.elements, port, cob);
              return SOCAN_ERROR;
            }
          for(;;)
            {
              rc_i= socan_hdl_wait_tmo(hdl, &ts);
              switch (rc_i)
                { case  1: return SOCAN_TIMEOUT;
                  case -1: return SOCAN_ERROR;
                }
              /* socan_hdl_wait_tmo also returns when the RTR frame was sent.
               * We simply ignore this here and wait again. Only when a reply
               * to the RTR frame arrived, obj->ircnt will change. */
              if (obj->urcnt != obj->ircnt)
                break; /* data has arrived */
            }
          break;
        case SOCAN_READ:
          if (obj->urcnt == obj->ircnt)
            { /* no data, must wait */
              calc_abs_timeout(obj->tmo_ms, &ts);
              for(;;)
                {
                  trace(1, "  read: wait on semaphore\n");
                  rc_i= socan_hdl_wait_tmo(hdl, &ts);
                  trace(1, "  read: semaphore wait returned: %d\n", rc_i);
                  if ((rc_i==1) || (rc_i==-1)) /* timeout or error */
                    break;
                  if (obj->urcnt != obj->ircnt)
                    break;
                } 
              switch (rc_i)
                { case  1: return SOCAN_TIMEOUT;
                  case -1: return SOCAN_ERROR;
                }
            }
          break;
        default:
          ERRPRINT(2, "cob %d is not a read object\n", cob);
          return SOCAN_NOT_RO_ERR;
      }
 
    /* from here we know that urcnt != ircnt : */
    /* read until ircnt hasn't changed, only then 
     * we know we have consistent data */
    do {
         ircnt= obj->ircnt;
         rlen= obj->received_length;
         memcpy(data, obj->buffer, obj->length);
       } while (ircnt != obj->ircnt);
    for(;;)
      {
        if (rlen != obj->length)
          {
            rc= SOCAN_LENGTH_ERR;
            break;
          }
        if (obj->urcnt == ircnt)
          {
            rc= SOCAN_OLD;
            break;
          }
        if ((unsigned short)(ircnt - obj->urcnt) > 1)
          {
            rc= SOCAN_LOST;
            break;
          }
        rc= SOCAN_OK;
        break;
      }
    obj->urcnt= ircnt;
    return rc;
  }
 
socan_rc socan_queue_read(socan_hdl hdl, unsigned char *port,
                          unsigned short *cob, void *data)
  {
    int ircnt;
    unsigned char rlen;
    struct socan_obj_s *obj= NULL;
    unsigned short cob_;
    unsigned char port_;
    can_port *cp;
    socan_rc rc;
 
    for(;;)
      {
        squeue_get(&(hdl->qread_queue), &cob_, &port_);
        if (!check_get_port(port_, __func__, &cp))
          continue;
        rc= check_get_cob(cob_, __func__, cp, &obj, true);
        if (rc!=SOCAN_OK)
          return rc;
        if (!obj->use_queue)
          {
            ERRPRINT(2, "not a qread object: %d port %d\n", cob_, port_);
            continue;
          }
 
        if (obj->urcnt == obj->ircnt)
          /* no new data in this object */
          continue;
        /* from here we know that urcnt != ircnt : */
        /* read until ircnt hasn't changed, only then 
         * we know we have consistent data */
        do {
             ircnt= obj->ircnt;
             rlen= obj->received_length;
             memcpy(data, obj->buffer, obj->length);
           } while (ircnt != obj->ircnt);
        *port= port_;
        *cob= cob_;
        trace(1, "  queue_read: port:%d cob:%d urcnt:%d ircnt:%d\n",
              port_, cob_, obj->urcnt, ircnt);
        for(;;)
          {
            if (rlen != obj->length)
              {
                rc= SOCAN_LENGTH_ERR;
                break;
              }
            if ((unsigned short)(ircnt - obj->urcnt) > 1)
              {
                rc= SOCAN_LOST;
                break;
              }
            rc= SOCAN_OK;
            break;
          }
        obj->urcnt= ircnt;
        return rc;
      }
  }
 
socan_rc socan_write(socan_hdl hdl,
                     unsigned char port, unsigned short cob, const void *data)
  /* this function may also be used to write data that will be fetched by some
   * other CAN-device by a rtr-request
   * may return:
   * SOCAN_ERROR       : generic error
   * SOCAN_OK          : data was read
   * SOCAN_TIMEOUT     : timeout, no data was sent 
   */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    int rc_i;
    struct timespec ts;
    socan_rc rc;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (rc!=SOCAN_OK)
      return rc;
 
    if ((obj->type != SOCAN_WRITE) && (obj->type != SOCAN_WRITE_RTR) && 
        (obj->type != SOCAN_READ_RTR))
      { /* neither SOCAN_WRITE nor SOCAN_WRITE_RTR object */
        ERRPRINT(2, "cob %d is not a write or RTR-read object\n", cob);
        return SOCAN_TYPE_ERR;
      }
 
    if (obj->type == SOCAN_WRITE_RTR)
      { /* special handling of writing to a SOCAN_WRITE_RTR object:
           we only write to the local buffer in this case. */
        tv_now(&(obj->tv_request));
        memcpy(canobj_rtr_wbuf_other(obj), data, obj->length);
        /* switch between two buffers to avoid race condition with the writer
         * task : */
        canobj_rtr_switch(obj);
        return SOCAN_OK;
      }
 
    /* the following code also handles writing an RTR request frame when the
     * object type is SOCAN_READ_RTR. */
 
    tv_now(&(obj->tv_request));
    obj->ulock= 1; /* mark that we are writing */
    trace(1, "tm bef. write: %lu\n", tv_to_ms(&(obj->tv_request))); 
    memcpy(obj->buffer, data, obj->length);
    obj->w_echo= 0;
    obj->ulock= 0; /* mark that we are finished with writing */
    if (!squeue_add(&(cp->writer_squeue), cob, port))
      {
        ERRPRINT(3, "squeue_add failed, queue full "
                    "(size %d) port:%d cob:%d\n",
                 cp->writer_squeue.elements, port, cob);
        return SOCAN_ERROR;
      }
    calc_abs_timeout(obj->tmo_ms, &ts);
    for(;;)
      {
        rc_i= socan_hdl_wait_tmo(hdl, &ts);
        if ((rc_i==1) || (rc_i==-1)) /* timeout or error */
          break;
        if (obj->w_echo)
          break;
      } 
    trace(1, "tm aft. write: %lu\n", tm_ms_now()); 
    switch (rc_i)
      { case  1: return SOCAN_TIMEOUT;
        case -1: return SOCAN_ERROR;
        default: return SOCAN_OK;
      }
  }
 
socan_rc socan_writelater(socan_hdl hdl,
                          unsigned char port, unsigned short cob, const void *data)
  /* Write a message on the can bus, do not wait for a result,
   * This function may also be used to create an RTR request frame by writing
   * to an SOCAN_READ_RTR object.
   *
   * other CAN-device by a rtr-request
   * may return:
   * SOCAN_ERROR       : generic error
   * SOCAN_WAIT        : previous write hasn't happened yet, write rejected
   * SOCAN_OK          : data was writen
   * SOCAN_TIMEOUT     : previous write had a timeout
   * SOCAN_TYPE_ERR    : wrong type of CAN object
   */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc my_rc= SOCAN_OK;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    my_rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (my_rc!=SOCAN_OK)
      return my_rc;
 
    if ((obj->type != SOCAN_WRITE) && (obj->type != SOCAN_WRITE_RTR) && 
        (obj->type != SOCAN_READ_RTR))
      { /* not SOCAN_WRITE, SOCAN_WRITE_RTR or SOCAN_WRITE_RTR object */
        ERRPRINT(2, "cob %d is not a write or RTR-read object\n", cob);
        return SOCAN_TYPE_ERR;
      }
 
    if (obj->type == SOCAN_WRITE_RTR)
      { /* special handling of writing to a SOCAN_WRITE_RTR object:
           we only write to the local buffer in this case. */
        tv_now(&(obj->tv_request));
        memcpy(canobj_rtr_wbuf(obj), data, obj->length);
        /* switch between two buffers to avoid race condition with the writer
         * task : */
        canobj_rtr_switch(obj);
        return SOCAN_OK;
      }
 
    /* the following code also handles writing an RTR request frame when the
     * object type is SOCAN_READ_RTR. */
 
    if (!obj->w_echo)
      { /* old request still running */
        unsigned long tdelta= tm_ms_diff_to_now(&(obj->tv_request));
        if (tdelta < obj->tmo_ms)
          { /* old request still running, no timeout yet */
            return SOCAN_WAIT;
          }
        /* from here: previous write must have had timeout */
        my_rc= SOCAN_TIMEOUT;
      }
 
    tv_now(&(obj->tv_request));
    obj->ulock= 1; /* mark that we are writing */
    memcpy(obj->buffer, data, obj->length);
    obj->w_echo= 0;
    obj->ulock= 0; /* mark that we are finished with writing */
    if (!squeue_add(&(cp->writer_squeue), cob, port))
      {
        ERRPRINT(3, "squeue_add failed, queue full "
                    "(size %d) port:%d cob:%d\n",
                 cp->writer_squeue.elements, port, cob);
        return SOCAN_ERROR;
      }
    return my_rc;
  }
 
socan_rc socan_write_inhibit(socan_hdl hdl,
                             unsigned char port, unsigned short cob,
                             const void *data,
                             unsigned long *inhibit_time)
  /* write a message on the can bus, do not wait for a result,
   * this function may also be used to write data that will be fetched by some
   * other CAN-device by a rtr-request
   * may return:
   * SOCAN_ERROR       : generic error
   * SOCAN_WAIT        : previous write hasn't happened yet, write rejected
   * SOCAN_OK          : data was writen
   * SOCAN_TIMEOUT     : previous write had a timeout
   */
  {
    can_port *cp;
    struct socan_obj_s *obj;
    socan_rc my_rc= SOCAN_OK;
    unsigned long tm_us_next_write;
    unsigned long tm_us_now_;
 
    if (!check_get_port(port, __func__, &cp))
      return SOCAN_PORT_ERR;
    my_rc= check_get_cob(cob, __func__, cp, &obj, true);
    if (my_rc!=SOCAN_OK)
      return my_rc;
 
    if ((obj->type != SOCAN_WRITE) && (obj->type != SOCAN_WRITE_RTR) && 
        (obj->type != SOCAN_READ_RTR))
      { /* not SOCAN_WRITE, SOCAN_WRITE_RTR or SOCAN_WRITE_RTR object */
        ERRPRINT(2, "cob %d is not a write or RTR-read object\n", cob);
        return SOCAN_TYPE_ERR;
      }
 
    /* initialize inhibit_time with 0: */
    *inhibit_time= 0;
    if (obj->type == SOCAN_WRITE_RTR)
      { /* special handling of RTR write */
        tv_now(&(obj->tv_request));
        memcpy(canobj_rtr_wbuf(obj), data, obj->length);
        /* switch between two buffers to avoid race condition with the writer
         * task : */
        canobj_rtr_switch(obj);
        return SOCAN_OK;
      }
 
    /* the following code also handles writing an RTR request frame when the
     * object type is SOCAN_READ_RTR. */
    if (!obj->w_echo)
      { /* old request still running */
        unsigned long tdelta= tm_ms_diff_to_now(&(obj->tv_request));
        if (tdelta < obj->tmo_ms)
          { /* old request still running, no timeout yet */
            /* must wait (at least) the full inhibit time: */
            *inhibit_time= obj->tm_inhibit_us;
            return SOCAN_WAIT;
          }
        /* from here: previous write had timeout */
        my_rc= SOCAN_TIMEOUT;
      }
    /* wait-time= timestamp+inhibit-now */
    tm_us_next_write= tv_to_us(&(obj->tv)) + obj->tm_inhibit_us;
    tm_us_now_= tm_us_now();
    if (tm_us_now_ < tm_us_next_write)
      { /* must wait */
        *inhibit_time= tm_us_next_write - tm_us_now_;
        return SOCAN_WAIT; /* hides possible SOCAN_TIMEOUT */
      }
    /* we can continue here: */
    obj->ulock= 1; /* mark that we are writing */
    tv_now(&(obj->tv_request));
    memcpy(obj->buffer, data, obj->length);
    obj->w_echo= 0;
    obj->ulock= 0; /* mark that we are finished with writing */
    if (!squeue_add(&(cp->writer_squeue), cob, port))
      {
        ERRPRINT(3, "squeue_add failed, queue full "
                    "(size %d) port:%d cob:%d\n",
                 cp->writer_squeue.elements, port, cob);
        return SOCAN_ERROR;
      }
    return my_rc;
  }
 
/* ----------------------------------------------
 * reader and writer threads
 * ---------------------------------------------- */
 
static int init_can_socket(can_port *cp, int *sock)
  {
    int rc, s;
    struct sockaddr_can addr;
    int interface_index;
    int     sval;
 
    *sock= 0;
    if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
      {
        ERRPRINT_ERRNO("socket");
        return 1;
      }
    sval= SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_TX_HARDWARE;
    if ((rc= setsockopt(s, SOL_SOCKET, SO_TIMESTAMPING, &sval, sizeof(sval))))
      {
        ERRPRINT_RC("setsockopt", rc);
        return 2;
      }
    interface_index= can_port_interface_index(cp, s);
    if (interface_index==-1)
      {
        ERRPRINT(1, "can_port_interface_index failed\n");
        return 20;
      }
 
    memset(&addr, 0, sizeof(addr));
    addr.can_family = AF_CAN;
    addr.can_ifindex = interface_index;
    /* printf("ifr_index: %d\n", interface_index); */
 
    if (bind(s, (struct sockaddr *) &addr, sizeof(addr)) < 0)
      {
        ERRPRINT_ERRNO("bind");
        return 3;
      }
    *sock= s;
    return 0;
  }
 
#define TASK "reader"
static void *reader(void *arg)
  /* return code !=0 on error */
  {
    can_port *cp= (can_port *)arg;
    unsigned char port= ((can_port *)arg)->port;
    int s;
    int is_rtr;
    struct can_frame frame;
    static int read_can_device_rc= 0;
    socan_rc rc;
 
    read_can_device_rc= init_can_socket(cp, &s);
    if (read_can_device_rc!=0)
      {
        ERRPRINT(1, "init_can_socket failed: %d\n", read_can_device_rc);
        pthread_exit(NULL);
      }
 
    MUTEX_LOCK(&global_semaphore);
    thread_count++;
    MUTEX_UNLOCK(&global_semaphore);
 
    for(;;)
      {
        int nbytes = read(s, &frame, sizeof(struct can_frame));
        unsigned short cob;
        struct socan_obj_s *obj;
 
        if (nbytes < 0)
          {
            ERRPRINT_ERRNO("read");
            /* continue in spite of error */
            continue;
          }
        cob= frame.can_id & CAN_SFF_MASK; /* mask out special bits */
        is_rtr= (frame.can_id & CAN_RTR_FLAG) ? 1 : 0;
        rc= check_get_cob(cob, __func__, cp, &obj, false);
        if (rc!=SOCAN_OK)
          continue;
 
        /* do nothing when handle is not active: */
        if (!obj->hdl_s->is_active)
          continue;
 
        if (obj->ulock)
          { /* user task is writing (write, but no RTR write) to the object
               right now, we skip everything here */
            trace(2, "(%s) ulock ! port:%d cob:0x%03X RTR:%1d",
                  TASK, port, cob, is_rtr);
            continue;
          };
 
        /* get timestamp: */
        ioctl(s, SIOCGSTAMP, &(obj->tv));
 
        switch(obj->type)
          {
            case SOCAN_READ:
              if (is_rtr)
                {
                  ERRPRINT(3, "  (%s) RTR on SOCAN_READ "
                              "object received "
                              "port:%d cob:0x%03X RTR:%1d\n",
                              TASK, port, cob, is_rtr);
                  continue;
                }
              /* mark changes: */
              (obj->ircnt)++;
              memcpy(obj->buffer, frame.data, frame.can_dlc);
              obj->received_length= frame.can_dlc;
              if (obj->use_queue)
                {
                  if (!squeue_add(&(obj->hdl_s->qread_queue), cob, port))
                    {
                      ERRPRINT(3, "(task %s) "
                                  "squeue_add failed, queue full "
                                  "(size %d) port:%d cob:%d RTR:%1d\n",
                               TASK,
                               cp->writer_squeue.elements, port, cob, is_rtr);
                      break;
                    }
                }
              socan_hdl_give(obj->hdl_s);
              break;
            case SOCAN_READ_RTR:
              if (is_rtr)
                { /* echo of RTR frame sent by this host, 
                     mark changes: */
                  if (!obj->ulock)
                    {
                      obj->w_echo= 1;
                      socan_hdl_give(obj->hdl_s);
                    }
                }
              else
                {
                  /* data arrived as reply to an RTR request,
                     mark changes: */
                  (obj->ircnt)++;
                  memcpy(obj->buffer, frame.data, frame.can_dlc);
                  obj->received_length= frame.can_dlc;
                  if (obj->use_queue)
                    {
                      if (!squeue_add(&(obj->hdl_s->qread_queue), cob, port))
                        {
                          ERRPRINT(3, "(task %s) "
                                      "squeue_add failed, queue full "
                                      "(size %d) port:%d cob:%d RTR:%1d\n",
                                   TASK,
                                   cp->writer_squeue.elements,
                                   port, cob, is_rtr);
                          break;
                        }
                    }
                  socan_hdl_give(obj->hdl_s);
                }
              break;
            case SOCAN_WRITE_RTR:
              if (!is_rtr)
                {
                  ERRPRINT(3, "  (%s) non-RTR on SOCAN_WRITE_RTR "
                              "object received "
                              "port:%d cob:0x%03X RTR:%1d\n",
                              TASK, port, cob, is_rtr);
                  continue;
                }
              /* an RTR request was received by another host, create an
                 answer by enqueuing a send request for writer task: */
              if (!squeue_add(&(cp->writer_squeue), cob, port))
                {
                  ERRPRINT(3, "(task %s) "
                              "squeue_add failed, queue full "
                              "(size %d) port:%d cob:%d RTR:%1d\n",
                           TASK,
                           cp->writer_squeue.elements, port, cob, is_rtr);
                  continue;
                }
              trace(2, "  (%s) create reply on received "
                       "RTR request "
                       "port:%d cob:0x%03X RTR:%1d\n",
                       TASK, port, cob, is_rtr);
              continue;
            case SOCAN_WRITE:
              if (is_rtr)
                {
                  ERRPRINT(3, "  (%s) RTR on SOCAN_WRITE "
                              "object received "
                              "port:%d cob:0x%03X RTR:%1d\n",
                              TASK, port, cob, is_rtr);
                  continue;
                }
              /* we got the data of a "write" on the local host via
                 loopback, set w_echo and give the semaphore in case
                 "write" is waiting on it:
               */
              trace(2, "  (%s) local echo of CAN write received "
                       "port:%d cob:0x%03X RTR:%1d\n",
                       TASK, port, cob, is_rtr);
              /* change w_echo only when ulock is not set: */
              if (!obj->ulock)
                {
                  obj->w_echo= 1;
                  socan_hdl_give(obj->hdl_s);
                }
              continue;
          }
        if (tracelevel >= 2)
          trace_object(2, TASK, obj, port, cob, &frame);
      }
 
    if (close(s) < 0)
      {
        ERRPRINT_ERRNO("close");
        read_can_device_rc= 5;
      }
    pthread_exit(NULL);
  }
#undef TASK
 
#define TASK "writer"
static void *writer(void *arg)
  {
    can_port *cp= (can_port *)arg;
    unsigned char writer_port= cp->port;
    int s;
    static int write_can_device_rc= 0;
    struct can_frame frame;
    int i;
    int can_frame_sz= sizeof(struct can_frame);
    socan_rc rc;
 
    /* initialize frame variable with 0 bytes: */
    memset(&frame, 0, sizeof(struct can_frame));
 
    write_can_device_rc= init_can_socket(cp, &s);
 
    if (write_can_device_rc!=0)
      {
        ERRPRINT(1, "init_can_socket failed: %d\n", write_can_device_rc);
        pthread_exit(NULL);
      }
    MUTEX_LOCK(&global_semaphore);
    thread_count++;
    MUTEX_UNLOCK(&global_semaphore);
 
    for(;;)
      {
        unsigned short cob;
        unsigned char  port;
        struct socan_obj_s *obj;
 
        squeue_get(&(cp->writer_squeue), &cob, &port);
        if (writer_port != port)
          {
            ERRPRINT(3, "port %d != %d\n", port, writer_port);
            continue;
          }
 
        rc= check_get_cob(cob, __func__, cp, &obj, true);
        if (rc!=SOCAN_OK)
          continue;
 
        /* do nothing when handle is not active: */
        if (!obj->hdl_s->is_active)
          continue;
 
        switch(obj->type)
          {
            case SOCAN_WRITE_RTR:
              /* must answer RTR request */
              frame.can_id= cob;
              memcpy(frame.data, canobj_rtr_wbuf(obj), obj->length);
              frame.can_dlc= obj->length;
              break;
            case SOCAN_READ_RTR:
              /* must send RTR request */
              frame.can_id= cob | CAN_RTR_FLAG;
              memset(frame.data, 0, obj->length);
              frame.can_dlc= obj->length;
              break;
            default:
              if (obj->ulock)
                { /* User task is currently writing to the data field. It may
                     contain inconsistent data so we discard *this* write
                     request */
                  continue;
                };
 
              frame.can_id= cob;
              memcpy(frame.data, obj->buffer, obj->length);
              frame.can_dlc= obj->length;
              break;
          }
 
        trace(2, "(%s) port:%d cob:0x%03X ", TASK, port, cob);
        trace(2, "data: ");
        for (i = 0; i < frame.can_dlc; i++)
          trace(2, "%02X ", frame.data[i]);
        trace(2, "\n");
 
        if (write(s, &frame, can_frame_sz) != can_frame_sz)
          {
            ERRPRINT_ERRNO("write");
          }
      }
  }
#undef TASK
 
/* ----------------------------------------------
 * setup module
 * ---------------------------------------------- */
 
bool socan_realtime_setup(bool use_rt_priority,
                          bool require_rt_priority,
                          int reader_priority,
                          int writer_priority)
  /* Set up of some defaults. If used, must be called before socan_init().
   *
   * writer_priority and reader_priority are only used when real time
   * scheduling is used (use_rt_priority==true).
   *
   * Note: socan_realtime_setup is not strictly thread safe, if socan_init() is
   * started at the same time as this function, this may cause problems. */
  {
    int min_prio, max_prio;
 
    switch (socan_state)
      {
        case ERROR:
             ERRPRINT(1, "socan is in ERROR state, cannot setup realtime\n");
             return false;
        case INITIALIZED:
             ERRPRINT(1, "socan is already initialized, "
                         "cannot setup realtime\n");
             return false;
        case UNINITIALIZED:
             break;
        default:
             ERRPRINT(1, "unknown internal state: %d\n", socan_state);
             return false;
      }
 
    if (use_rt_priority)
      {
        min_prio= sched_get_priority_min(SCHED_FIFO);
        max_prio= sched_get_priority_max(SCHED_FIFO);
        if ((reader_priority < min_prio) || (reader_priority > max_prio))
          {
            ERRPRINT(1, "Error, reader priority %d out of range "
                        "%d .. %d\n",
                        reader_priority, min_prio, max_prio);
            return false;
          }
        if ((writer_priority < min_prio) || (writer_priority > max_prio))
          {
            ERRPRINT(1, "Error, writer priority %d out of range "
                        "%d .. %d\n",
                        writer_priority, min_prio, max_prio);
            return false;
          }
      }
 
    socan_use_rt_priority= use_rt_priority;
    socan_require_rt_priority= require_rt_priority;
    if (use_rt_priority)
      {
        socan_reader_priority= reader_priority;
        socan_writer_priority= writer_priority;
      }
    return true;
  }
 
/* ----------------------------------------------
 * init module
 * ---------------------------------------------- */
 
_Atomic static int _lock= 0;
 
bool socan_init(void)
  /* generic initialization */
  {
    bool ok;
    int p, n;
 
    /* note: socan_init is not strictly thread safe, if started at the same
     * time from two threads this may cause problems. */
    if (socan_state == INITIALIZED)
      return true;
    if (socan_state == ERROR)
      return false;
    if (can_ports_no==0)
      {
        ERRPRINT(1, "no ports are defined\n");
        return false;
      }
    /* reading and writing to _lock is atomic: */
    if (_lock++)
      {
        /* some other thread is initializing, check every 10ms if it is
         * finished */
        while (socan_state==UNINITIALIZED)
          mldelay(10);
        if (socan_state == INITIALIZED)
          return true;
        if (socan_state == ERROR)
          return false;
      }
 
    /* *this* thread is now exclusively initializing: */
    if (!mutex_init(&global_semaphore))
      {
        ERRPRINT(1, "mutex_init failed\n");
        return false;
      }
 
    /* initialize: */
    MUTEX_LOCK(&global_semaphore);
 
    for (p=0; p<can_ports_no; p++)
      {
        /* read thread: */
        if (!thread_start(reader, &(socan_read_threads[p]), 
                          thread_suffix(true, p),
                          8192,                  /* stack */
                          socan_reader_priority, /* prio */
                          &(can_ports[p])))      /* arg */
          {
            ERRPRINT(1, "thread_start (reader) failed\n");
            socan_state= ERROR;
            MUTEX_UNLOCK(&global_semaphore);
            return false;
          }
        /* write thread: */
        if (!thread_start(writer, &(socan_write_threads[p]), 
                          thread_suffix(false, p),
                          8192,                   /* stack */
                          socan_writer_priority,  /* prio */
                          &(can_ports[p])))       /* arg */
          {
            ERRPRINT(1, "thread_start (writer) failed\n");
            socan_state= ERROR;
            MUTEX_UNLOCK(&global_semaphore);
            return false;
          }
      }
    /* must wait for all threads to start : */
    socan_state= INITIALIZED;
    MUTEX_UNLOCK(&global_semaphore);
    ok= false;
    mldelay(50); /* initial sleep of 50 ms */
    for(n=can_ports_no;n>0;n--) /* 100ms for each port to initialize */
      {
        mldelay(100); /* sleep 100 ms */
        MUTEX_LOCK(&global_semaphore);
        if (thread_count >= can_ports_no*2)
          {
            ok= true;
            MUTEX_UNLOCK(&global_semaphore);
            break;
          }
        MUTEX_UNLOCK(&global_semaphore);
      }
    if (!ok)
      {
        socan_state= ERROR;
        ERRPRINT(1, "starting all threads took too long\n");
      }
    return ok;
  }