SocketCAN cyclic RT testbench

canbench.c

/* SocketCAN cyclic RT testbench

   compile using:
     ${CROSS_COMPILE}gcc -o canbench canbench.c -lrt -Wall
*/

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <stdint.h>
#include <time.h>
#include <sched.h>

#include <unistd.h>

#include <net/if.h>
#include <sys/types.h>
#include <sys/socket.h>
#include <sys/ioctl.h>
 
#include <linux/can.h>
#include <linux/can/raw.h>

#define NSEC_PER_SEC 1000000000

/* using clock_nanosleep of librt */
extern int clock_nanosleep(clockid_t __clock_id, int __flags,
      __const struct timespec *__req,
      struct timespec *__rem);

int can_socket;

/* the struct timespec consists of nanoseconds
 * and seconds. if the nanoseconds are getting
 * bigger than 1000000000 (= 1 second) the
 * variable containing seconds has to be
 * incremented and the nanoseconds decremented
 * by 1000000000.
 */
static inline void tsnorm(struct timespec *ts)
{
   while (ts->tv_nsec >= NSEC_PER_SEC) {
      ts->tv_nsec -= NSEC_PER_SEC;
      ts->tv_sec++;
   }
}

int canopen()
{
    struct sockaddr_can addr;
    struct ifreq ifr;

    char *ifname = "can0";

    if((can_socket = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
        perror("Error while opening can socket");
        return -1;
    }

    strcpy(ifr.ifr_name, ifname);

    ioctl(can_socket, SIOCGIFINDEX, &ifr);

    addr.can_family  = AF_CAN;
    addr.can_ifindex = ifr.ifr_ifindex; 

    printf("iface %s at index %d OK\n", ifname, ifr.ifr_ifindex);

    if(bind(can_socket, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
        perror("Error in socket bind");
        return -2;
    }
    
    return 0;
}

int cansend()
{
    struct can_frame frame;

    frame.can_id  = 0x266;
    frame.can_dlc = 8;
    frame.data[0] = 0x11;
    frame.data[1] = 0x22;
    frame.data[2] = 0x33;
    frame.data[3] = 0x44;
    frame.data[4] = 0x55;
    frame.data[5] = 0x66;
    frame.data[6] = 0x77;
    frame.data[7] = 0x88;

    write(can_socket, &frame, sizeof(struct can_frame));
    
    return 0;
}

int main(int argc,char** argv)
{
    struct timespec t;
    struct sched_param param;

    int interval = 1000000; // 1 000 000 = 1ms

    printf("# canbench [ <rt-prio> [ <interval-us> ] ]\n");

    int r = canopen();

    if (r) {
        return 2;
    }

    if (argc>=2 && atoi(argv[1])>0) {
       printf("using realtime, priority: %d\n",atoi(argv[1]));
       param.sched_priority = atoi(argv[1]);
       /* enable realtime fifo scheduling */
       if(sched_setscheduler(0, SCHED_FIFO, &param)==-1){
          perror("sched_setscheduler failed");
          exit(-1);
       }
    }

    if (argc>=3) {
       interval=1000*atoi(argv[2]);
    }

    printf("interval: %f ms\n", interval / 1.0e6 );
   
   /* get current time */
   clock_gettime(0,&t);

   while(1){
      /* wait untill next shot */
      clock_nanosleep(0, TIMER_ABSTIME, &t, NULL);

      /* do the stuff */
      cansend();

      /* calculate and normalize next shot */
      t.tv_nsec+=interval;
      tsnorm(&t);
   }

   return 0;
}