socketinterface.c

Go to the documentation of this file.

 
#ifndef _GNU_SOURCE  // needed for asprintf to avoid warning
#define _GNU_SOURCE 1
#endif
 
/*---- "module header" ----------------------------------------------------------------*/
#include "toposens/linux/socketinterface.h"
/*---- <system includes> --------------------------------------------------------------*/
#ifdef CAN_AVAILABLE
#include <linux/can/raw.h>
#include <net/if.h>
#include <pthread.h>
#include <semaphore.h>
#include <stdbool.h>
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <sys/ioctl.h>
#include <sys/socket.h>
#include <sys/time.h>
#include <unistd.h>
 
/*---- "library includes" -------------------------------------------------------------*/
 
/*---- "project includes" -------------------------------------------------------------*/
 
/*---- local macros and constants -----------------------------------------------------*/
#define SOCKET_TIMEOUT 1
 
#define SOCKET_TIMEOUT_US 0
/*---- local types --------------------------------------------------------------------*/
 
/*---- local functions prototypes -----------------------------------------------------*/
 
static void PrintCanFrame(struct can_frame* frame);
 
static int SetupInterface(char* InterfaceName_cp, uint32_t InterfaceBitrate_u32);
 
static void StartReceiverThread();
 
int ReceiverThreadShouldRunCAN_b();
 
static void* Receiver();
 
static struct can_frame ReadFrame();
/*---- local inline functions ---------------------------------------------------------*/
 
/*---- local variables (static) -------------------------------------------------------*/
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static int s = 0;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static pthread_t receiver_thread_id;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static bool ReceiverThreadShouldRun = false;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static sem_t socket_sem;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static sem_t thread_sem;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static void (*ReadCallback)(struct can_frame* frame) = NULL;
// NOLINTNEXTLINE(cppcoreguidelines-avoid-non-const-global-variables)
static pthread_mutex_t mutex_socket_receiver = PTHREAD_MUTEX_INITIALIZER;
 
static bool ConnectedToSocket_b = false;
/*---- public variables ---------------------------------------------------------------*/
 
/*---- functions ----------------------------------------------------------------------*/
 
void RegisterReadCallback(void (*Callback)(struct can_frame* frame))
{
  MPRINTF("RegisterReadCallback\n");
  ReadCallback = Callback;
}
 
static struct can_frame ReadFrame()
{
  struct can_frame frame = {0};
  if (read(s, &frame, sizeof(struct can_frame)) < 0)
  {
    frame.can_dlc = 0;
  }
  return frame;
}
 
int WriteFrame(struct can_frame* frame)
{
  MPRINTF("Write Frame\n");
  if (ConnectedToSocket_b)
  {
    MPRINTF("WriteFrame: \t");
    if (write(s, frame, sizeof(struct can_frame)) != sizeof(struct can_frame))
    {
      perror("Write");
      return 1;
    }
 
    PrintCanFrame(frame);
    return 0;
  }
  MPRINTF(
      "An attempt was made to send CAN messages via a socket that has not yet been configured.\n");
  return 1;
}
 
static void PrintCanFrame(struct can_frame* frame)
{
  MPRINTF("0x%03X [%d] ", frame->can_id, frame->can_dlc);
 
  for (int i = 0; i < frame->can_dlc; i++)
  {
    MPRINTF("%02X ", frame->data[i]);
  }
  MPRINTF("\r\n");
}
 
static int SetupInterface(char* InterfaceName_cp, uint32_t InterfaceBitrate_u32)
{
  MPRINTF("SetupInterface\n");
  char* init_command = NULL;
  char* down_command = NULL;
  if (asprintf(&init_command, "ip link set %s up type can bitrate %d dbitrate %d fd off",
               InterfaceName_cp, InterfaceBitrate_u32, InterfaceBitrate_u32) == -1)
  {
    return -1;
  }
  if (asprintf(&down_command, "ifconfig %s down", InterfaceName_cp) == -1)
  {
    return 1;
  }
  if (getuid() == 0)
  {
    // NOLINTNEXTLINE(cert-env33-c): Using Systemcall is currently the quickest way to get this
    // done. Since the usage of "system" is potential dangerous, I might try to find a workaround
    if (system(init_command) != 0)
    {
      MPRINTF(
          "Interface Setup failed on first try.\nTrying to disable can0 interface and try "
          "again.\n");
      MPRINTF("%s\n", init_command);
      MPRINTF("%s\n", down_command);
      // NOLINTNEXTLINE(cert-env33-c): Using Systemcall is currently the quickest way to get this
      // done. Since the usage of "system" is potential dangerous, I might try to find a workaround
      if (system(down_command) == 0)
      {
        // NOLINTNEXTLINE(cert-env33-c): Using Systemcall is currently the quickest way to get this
        // done. Since the usage of "system" is potential dangerous, I might try to find a
        // workaround
        if (system(init_command) != 0)
        {
          free(init_command);
          free(down_command);
          return 1;
        }
      }
      else
      {
        free(init_command);
        free(down_command);
        return 1;
      }
    }
    free(init_command);
    free(down_command);
    return 0;
  }
  MPRINTF("---------------Warning---------------\n");
  MPRINTF(
      "Current User is not root - will not attempt to configure the interface. Please take care of "
      "proper interface setup yourself!\n");
  MPRINTF("-------------------------------------\n");
  free(init_command);
  free(down_command);
  return 0;
}
 
int SetupSocket(char* InterfaceName_cp, uint32_t InterfaceBitrate_u32)
{
  MPRINTF("SetupSocket for interface %s / %d\n", InterfaceName_cp, InterfaceBitrate_u32);
  if (SetupInterface(InterfaceName_cp, InterfaceBitrate_u32) == 0)
  {
    MPRINTF("Interface setup successful\n");
  }
  else
  {
    MPRINTF("Interface setup failed\n");
    return 1;
  }
  struct sockaddr_can addr = {0};
  struct ifreq ifr;
  struct timeval tv;
  tv.tv_sec = SOCKET_TIMEOUT;
  tv.tv_usec = SOCKET_TIMEOUT_US;
 
  if ((s = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0)
  {
    perror("Socket");
    return 1;
  }
  setsockopt(s, SOL_SOCKET, SO_RCVTIMEO, (const char*)&tv, sizeof tv);
  strncpy(ifr.ifr_name, "can0", IFNAMSIZ);
  ioctl(s, SIOCGIFINDEX, &ifr);
 
  addr.can_family = AF_CAN;
  addr.can_ifindex = ifr.ifr_ifindex;
 
  if (bind(s, (struct sockaddr*)&addr, sizeof(addr)) < 0)
  {
    perror("Bind");
    return 1;
  }
  sem_init(&socket_sem, 0, 1);
  sem_init(&thread_sem, 0, 1);
  pthread_mutex_lock(&mutex_socket_receiver);
  ReceiverThreadShouldRun = true;
  pthread_mutex_unlock(&mutex_socket_receiver);
  sem_wait(&thread_sem);
  StartReceiverThread();
  sem_wait(&thread_sem);
  sem_post(&thread_sem);
 
  ConnectedToSocket_b = true;
  return 0;
}
 
int DeinitSocket()
{
  if (close(s) < 0)
  {
    perror("Close");
    return 1;
  }
  pthread_mutex_lock(&mutex_socket_receiver);
  ReceiverThreadShouldRun = false;
  pthread_mutex_unlock(&mutex_socket_receiver);
  (void)pthread_join(receiver_thread_id, NULL);
  return 0;
}
 
static void StartReceiverThread()
{
  MPRINTF("StartReceiverThread\n");
  int err = 0;
  err = pthread_create(&(receiver_thread_id), NULL, &Receiver, NULL);
  if (err != 0)
  {
    MPRINTF("\ncan't create thread :[%s]", strerror(err));
  }
  else
  {
    MPRINTF("\nCAN-Receiver Thread created successfully\n");
  }
  sem_post(&thread_sem);
}
 
int ReceiverThreadShouldRunCAN_b()
{
  int value = 0;
 
  pthread_mutex_lock(&mutex_socket_receiver);
  value = ReceiverThreadShouldRun;
  pthread_mutex_unlock(&mutex_socket_receiver);
 
  return value;
}
 
static void* Receiver()
{
  MPRINTF("Start Receiver\n");
  while (ReceiverThreadShouldRunCAN_b())
  {
    MPRINTF("Reading now:\n");
    struct can_frame frame = ReadFrame();
    if (frame.can_dlc > 0)
    {
      if (ReadCallback != NULL)
      {
        ReadCallback(&frame);
      }
    }
  }
  MPRINTF("End Receiver\n");
  return NULL;
}
#endif