can.cpp

#include "can.h"
#include "commexceptions.h"
#include <string.h>
#include <iostream>
#include <math.h>
#include <errno.h>

CCAN::CCAN(const std::string &comm_id) : CComm(comm_id)
{
  this->can_socket_fd=-1;
  this->req_can_id=-1;
  this->rx_filters=NULL;
  this->num_filters=0;
  // initialize the thread attributes
  this->thread_server=CThreadServer::instance();
  this->can_thread_id=comm_id + "_can_thread";
  this->thread_server->create_thread(this->can_thread_id);
  this->thread_server->attach_thread(this->can_thread_id,this->can_thread,this);
  // initialize the event atributes
  this->event_server=CEventServer::instance();
  this->new_frame_event_id=comm_id + "_new_frame_event_id";
  this->event_server->create_event(this->new_frame_event_id);
  this->frame_error_event_id=comm_id + "_frame_error_event_id";
  this->event_server->create_event(this->frame_error_event_id);
  this->data_requested_event_id=comm_id + "_data_requested_event_id";
  this->event_server->create_event(this->data_requested_event_id);
  this->finish_can_thread_event_id=comm_id + "_finish_can_thread_event_id";
  this->event_server->create_event(this->finish_can_thread_event_id);
}

void CCAN::hard_open(void *comm_dev)
{
  std::string *can_dev=(std::string *)comm_dev;
  struct sockaddr_can addr;
  struct ifreq ifr;

  if ((this->can_socket_fd = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) 
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"impossible to create the socket-CAN",this->get_id());
  }
  addr.can_family = AF_CAN;
  strcpy(ifr.ifr_name, can_dev->c_str());
  ioctl(this->can_socket_fd, SIOCGIFINDEX, &ifr);
  addr.can_ifindex = ifr.ifr_ifindex;

  if (bind(this->can_socket_fd, (struct sockaddr *)&addr, sizeof(addr)) < 0) 
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"impossible to connect the socket-CAN",this->get_id());
  }
}

void CCAN::hard_config(void *config)
{
  // start the can thread
  this->thread_server->start_thread(this->can_thread_id);
}

int CCAN::hard_read(unsigned char *data, int len)
{
  int num_read=0;

  if(this->can_socket_fd!=-1)
  {
    if((num_read=::read(this->can_socket_fd,data,len))==-1)
    {
      /* handle exceptions */
      throw CCommException(_HERE_,"Error while reading from the socket.",this->comm_id);
    }
  }
  else
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"The socket-CAN has not been properly opened.",this->comm_id);
  }

  return num_read;
}

int CCAN::hard_write(unsigned char *data, int len)
{
  int num_write=0;

  if(this->can_socket_fd!=-1)
  {
    if((num_write=::write(this->can_socket_fd,data,len))==-1)
    {
      /* handle exceptions */
      throw CCommException(_HERE_,"Error while writing to the socket.",this->comm_id);
    }
  }
  else
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"The socket-CAN has not been properly opened.",this->comm_id);
  }

  return num_write;
}

int CCAN::hard_get_num_data(void)
{
  unsigned char data[1024];
  int num;

  if(this->can_socket_fd!=-1)
  {
    if((num=recv(this->can_socket_fd,data,1024,MSG_PEEK))==-1)
//    if(ioctl(this->can_socket_fd,FIONREAD,&num)==-1)
    {
      std::cout << errno << std::endl;
      /* handle exceptions */
      throw CCommException(_HERE_,"Error while getting the number of bytes available from the socket.",this->comm_id);
    }
  }
  else
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"The socket-CAN has not been properly opened.",this->comm_id);
  }

  return num;
}

int CCAN::hard_wait_comm_event(void)
{
  fd_set receive_set,error_set;
  int max_fd,wait_result=0;

  max_fd=this->can_socket_fd+1;
  FD_ZERO(&receive_set);
  FD_SET(this->can_socket_fd,&receive_set);
  FD_ZERO(&error_set);
  FD_SET(this->can_socket_fd,&error_set);
  wait_result=select(max_fd,&receive_set,NULL,&error_set,NULL);
  if(wait_result==-1)
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"Error while waiting for socket events",this->comm_id);
  }
  else
  {
    if(FD_ISSET(this->can_socket_fd,&receive_set))/* data has been received */
      return 1;
    if(FD_ISSET(this->can_socket_fd,&error_set))
      return 2;
  }

  return -1;
}

void CCAN::hard_close(void)
{
  // close the can device
  if(this->can_socket_fd!=-1)
    ::close(this->can_socket_fd);
}

void *CCAN::can_thread(void *param)
{
  struct can_frame new_frame;
  std::list<std::string> events;
  CCAN *can=(CCAN *)param;
  unsigned int event_id,num;
  bool end=false;

  events.push_back(can->finish_can_thread_event_id);
  events.push_back(can->get_rx_event_id());
  while(!end)
  {
    event_id=can->event_server->wait_first(events);
    if(event_id==0)
      end=true;
    else
    {
      can->can_access.enter();
      // process the incomming data
      num=((CComm *)can)->get_num_data();
      while(num>=sizeof(struct can_frame))// there is a whole frame in the input buffer
      {
        can->CComm::read((unsigned char *)&new_frame,sizeof(struct can_frame));
        if(new_frame.can_id&CAN_RTR_FLAG)
        {
          // save the requested can_id
          can->req_can_id=new_frame.can_id;
          // activate the event
          can->event_server->set_event(can->data_requested_event_id);
        }
        else
        {
          //push the new frame into the queue
          can->rx_frames.push(new_frame);
          can->can_data_map[new_frame.can_id]->write(new_frame.data,new_frame.can_dlc);
          //for (unsigned int i=0; i<new_frame.can_dlc; i++)
          //  can->can_data_map[new_frame.can_id].push_back(new_frame.data[i]);      
          // activate the event
          can->event_server->set_event(can->new_frame_event_id);
          can->event_server->set_event((std::to_string(new_frame.can_id) + "_new_data_event_id"));
        }
        num=((CComm *)can)->get_num_data();
        //num-=sizeof(struct can_frame);
      }
      can->can_access.exit();
    }
  }
 
  pthread_exit(NULL);
}

void CCAN::open(const std::string &can_dev)
{
  CComm::open((void *)&can_dev);
  this->config();
}

std::string CCAN::get_new_frame_event_id(void)
{
  return this->new_frame_event_id;
}

std::string CCAN::get_new_frame_event_id(unsigned int can_id)
{
  if(this->can_event_map.find(can_id)==this->can_event_map.end())
    throw CCommException(_HERE_,"Invalid can ID",this->comm_id);
  return this->can_event_map[can_id];
}

std::string CCAN::get_frame_error_event_id(void)
{
  return this->frame_error_event_id;
}

std::string CCAN::get_data_requested_event_id(void)
{
  return this->data_requested_event_id;
}

unsigned int CCAN::get_requested_can_id(void)
{ 
  return this->req_can_id;
}

unsigned long CCAN::get_last_rx_timestamp(void)
{ 
  struct timeval tv;
  
  if(ioctl(this->can_socket_fd,SIOCGSTAMP,&tv)==-1)
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"Impossible to get the last time stamp",this->comm_id);
  }

  return tv.tv_sec*1000000+tv.tv_usec;
}

void CCAN::write(unsigned int can_id, unsigned char *data, int len)
{
  int num_frames=ceil((double)len/8.0),i,j;
  struct can_frame frame;

  // check the identifier format
  if((can_id&(~CAN_SFF_MASK))!=0)// it's an extended identifier
    can_id=can_id|CAN_EFF_FLAG;

  for(i=0;i<num_frames;i++)
  {
    // generate all the frames
    frame.can_id=can_id;
    if((len-8*i)<8)
      frame.can_dlc=len-8*i;
    else
      frame.can_dlc=8;
    for(j=0;j<frame.can_dlc;j++)
      frame.data[j]=data[i*8+j];
    // send the frames
    this->CComm::write((unsigned char *)&frame,sizeof(struct can_frame));
  }
}

void CCAN::read(unsigned int can_id, unsigned char *data, int len)
{
  this->can_access.enter();
  if(!this->can_data_map[can_id]->is_empty())
  {
    this->can_data_map[can_id]->read(data,len);
  }
  else
  {
    /* handle exceptions */
    this->can_access.exit();    
    throw CCommException(_HERE_,"No data available",this->comm_id);
  }
  this->can_access.exit();
}

unsigned int CCAN::get_num_data(unsigned int can_id)
{
  unsigned int num_data;

  if(this->can_data_map.find(can_id)==this->can_data_map.end())
    throw CCommException(_HERE_,"Invalid can ID",this->comm_id);
 
  num_data = this->can_data_map[can_id]->get_num_data();
  
  return num_data;
}

std::string CCAN::add_id_filter(unsigned short int can_id)
{
  struct can_filter *new_filters;

  new_filters=new struct can_filter[this->num_filters+1];
  memcpy(new_filters,this->rx_filters,sizeof(struct can_filter)*this->num_filters);
  if(this->rx_filters!=NULL)
    delete[] this->rx_filters;
  this->rx_filters=new_filters;
  // update the new filter
  if((can_id&(~CAN_SFF_MASK))!=0)// it's an extended identifier
  {
    this->rx_filters[this->num_filters].can_id=can_id|CAN_EFF_FLAG;
    this->rx_filters[this->num_filters].can_mask=CAN_EFF_MASK;
  }
  else
  {
    this->rx_filters[this->num_filters].can_id=can_id;
    this->rx_filters[this->num_filters].can_mask=CAN_SFF_MASK;
  }
  //if(invert)
  //  this->rx_filters[this->num_filters].can_id|=CAN_INV_FILTER;
  // set up the new filters
  this->num_filters++;
  if(setsockopt(this->can_socket_fd,SOL_CAN_RAW,CAN_RAW_FILTER,this->rx_filters,sizeof(struct can_filter)*this->num_filters)==-1)
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"Error while setting up the filters",this->comm_id);
  }
  this->can_data_map[can_id]=new CQueue();
  this->can_event_map[can_id]=std::to_string(can_id) + "_new_data_event_id";
  this->event_server->create_event(this->can_event_map[can_id]);

  return this->can_event_map[can_id];
}

std::string CCAN::add_id_filter(unsigned short int can_id,unsigned short int mask)
{
  struct can_filter *new_filters;

  new_filters=new struct can_filter[this->num_filters+1];
  memcpy(new_filters,this->rx_filters,sizeof(struct can_filter)*this->num_filters);
  if(this->rx_filters!=NULL)
    delete[] this->rx_filters;
  this->rx_filters=new_filters;
  // update the new filter
  if((can_id&(~CAN_SFF_MASK))!=0)// it's an extended identifier
  {
    this->rx_filters[this->num_filters].can_id=can_id|CAN_EFF_FLAG;
    this->rx_filters[this->num_filters].can_mask=mask;
  }
  else
  {
    this->rx_filters[this->num_filters].can_id=can_id;
    this->rx_filters[this->num_filters].can_mask=mask;
  }
  //if(invert)
  //  this->rx_filters[this->num_filters].can_id|=CAN_INV_FILTER;
  // set up the new filters
  this->num_filters++;
  if(setsockopt(this->can_socket_fd,SOL_CAN_RAW,CAN_RAW_FILTER,this->rx_filters,sizeof(struct can_filter)*this->num_filters)==-1)
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"Error while setting up the filters",this->comm_id);
  }
  
  this->can_data_map[can_id]=new CQueue();
  this->can_event_map[can_id]=std::to_string(can_id) + "_new_data_event_id";
  this->event_server->create_event(this->can_event_map[can_id]);

  return this->can_event_map[can_id];
}

void CCAN::clear_id_filters(void)
{
  std::map<unsigned int,std::string>::iterator it;
  std::map<unsigned int,CQueue *>::iterator data_it;

  if(this->rx_filters!=NULL)
  {
    delete this->rx_filters;
    this->rx_filters=NULL;
    this->num_filters=0;
  }
  if(setsockopt(this->can_socket_fd,SOL_CAN_RAW,CAN_RAW_FILTER,this->rx_filters,sizeof(struct can_filter)*this->num_filters)==-1)
  {
    /* handle exceptions */
    throw CCommException(_HERE_,"Error while clearing the existing filters",this->comm_id);
  }

  for(it=this->can_event_map.begin();it!=this->can_event_map.end();it++)
    this->event_server->delete_event(it->second);
  for(data_it=this->can_data_map.begin();data_it!=this->can_data_map.end();data_it++)
    delete data_it->second;
}

void CCAN::close(void)
{
  if(this->can_socket_fd!=-1)
    CComm::close();
}

CCAN::~CCAN()
{
  std::map<unsigned int,std::string>::iterator it;
  std::map<unsigned int,CQueue *>::iterator data_it;

  // close the can device
  if(this->can_socket_fd!=-1)
    CComm::close();
  // free all the resources
  if(this->finish_can_thread_event_id!="")
  {
    this->event_server->set_event(this->finish_can_thread_event_id);
    this->thread_server->end_thread(this->can_thread_id);
    this->thread_server->detach_thread(this->can_thread_id);
    this->thread_server->delete_thread(this->can_thread_id);
    this->can_thread_id="";
    this->event_server->delete_event(this->finish_can_thread_event_id);
    this->finish_can_thread_event_id="";
  }
  if(this->new_frame_event_id!="")
  {
    this->event_server->delete_event(this->new_frame_event_id);
    this->new_frame_event_id="";
  }
  if(this->frame_error_event_id!="")
  {
    this->event_server->delete_event(this->frame_error_event_id);
    this->frame_error_event_id="";
  }
  if(this->data_requested_event_id!="")
  {
    this->event_server->delete_event(this->data_requested_event_id);
    this->data_requested_event_id="";
  }
  for(it=this->can_event_map.begin();it!=this->can_event_map.end();it++)
    this->event_server->delete_event(it->second);
  for(data_it=this->can_data_map.begin();data_it!=this->can_data_map.end();data_it++)
    delete data_it->second;
}