plugins/common/cansocketraw.cpp

   1 /*
   2 Copyright (C) 2012 Intel Corporation
   3
   4 This library is free software; you can redistribute it and/or
   5 modify it under the terms of the GNU Lesser General Public
   6 License as published by the Free Software Foundation; either
   7 version 2.1 of the License, or (at your option) any later version.
   8
   9 This library is distributed in the hope that it will be useful,
  10 but WITHOUT ANY WARRANTY; without even the implied warranty of
  11 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  12 Lesser General Public License for more details.
  13
  14 You should have received a copy of the GNU Lesser General Public
  15 License along with this library; if not, write to the Free Software
  16 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  17 */
  18
  19 #include <string.h>
  20 #include <sys/ioctl.h>
  21 #include <unistd.h>
  22 #include <sys/socket.h>
  23
  24 #include "logger.h"
  25 #include "cansocket.h"
  26 #include "cansocketraw.h"
  27 #include "timestamp.h"
  28
  29 CANSocketRaw::CANSocketRaw() :
  30     mSocket(-1)
  31 {
  32     LOG_TRACE("");
  33 }
  34
  35 bool CANSocketRaw::start(const char* ifName)
  36 {
  37     LOG_TRACE("");
  38
  39     if(mSocket < 0) {
  40         if(!createSocket()) {
  41             LOG_ERROR("Socket error");
  42         } else {
  43             can_err_mask_t errorMask = 0xFFFFFFFF;
  44             if(!enableCANErrors(errorMask)) {
  45                 LOG_ERROR("Socket error");
  46             } else
  47                 if(!enableTimestamps()) {
  48                     LOG_ERROR("Socket error");
  49                 } else {
  50                     mPoll.fd = mSocket;
  51                     mPoll.events = POLLIN | POLLPRI;
  52                     struct ifreq ifr;
  53                     memset(&ifr, 0, sizeof(ifr));
  54                     strcpy(ifr.ifr_name, ifName);
  55                     if(!locateInterfaceIndex(ifr)) {
  56                         LOG_ERROR("Socket error");
  57                         stop();
  58                     } else {
  59                         struct sockaddr_can addr;
  60                         memset(&addr, 0, sizeof(addr));
  61                         addr.can_family = AF_CAN;
  62                         addr.can_ifindex = ifr.ifr_ifindex;
  63                         if(!bindSocket(addr)) {
  64                             LOG_ERROR("Socket error");
  65                             stop();
  66                         } else {
  67                             return true;
  68                         }
  69                     }
  70                 }
  71         }
  72     }
  73     return false;
  74 }
  75
  76 void CANSocketRaw::stop()
  77 {
  78     LOG_TRACE("");
  79
  80     if(mSocket >= 0) {
  81         closeSocket();
  82         mSocket = -1;
  83     }
  84 }
  85
  86 bool CANSocketRaw::write(const struct CANFrameInfo &message)
  87 {
  88     LOG_TRACE("");
  89
  90     return writeFrame(message.frame);
  91 }
  92
  93 CANSocket::CANSocketReadSuccess CANSocketRaw::read(struct CANFrameInfo& message, unsigned int timeout)
  94 {
  95     LOG_TRACE("timeout: " << timeout);
  96
  97     CANSocket::CANSocketReadSuccess success;
  98     memset(&message, 0, sizeof(message));
  99
 100     switch(waitData(timeout)) {
 101     case -1:
 102         LOG_ERROR("reading error");
 103         success = CANSocket::READING_FAILED;
 104         break;
 105     case 0:
 106         success = CANSocket::READING_TIMED_OUT;
 107         break;
 108     default:
 109         ssize_t nbytes = (int)readFrame(message.frame, message.timestamp);
 110         message.status = CANFrameInfo::CANMessageStatus::GOOD;
 111         success = nbytes > 0 ? CANSocket::READING_SUCCEEDED : CANSocket::READING_FAILED;
 112     }
 113
 114     return success;
 115 }
 116
 117 bool CANSocketRaw::createSocket()
 118 {
 119     return ((mSocket = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) >= 0);
 120 }
 121
 122 bool CANSocketRaw::enableTimestamps()
 123 {
 124     const int timestamp = 1;
 125
 126     return (setsockopt(mSocket, SOL_SOCKET, SO_TIMESTAMP, &timestamp, sizeof(timestamp)) == 0);
 127 }
 128
 129 bool CANSocketRaw::enableCANErrors(can_err_mask_t errorMask)
 130 {
 131     return (setsockopt(mSocket, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &errorMask, sizeof(errorMask)) == 0);
 132 }
 133
 134 bool CANSocketRaw::locateInterfaceIndex(struct ifreq& ifr)
 135 {
 136     return (::ioctl(mSocket, SIOCGIFINDEX, &ifr) == 0);
 137 }
 138
 139 bool CANSocketRaw::bindSocket(struct sockaddr_can& addr)
 140 {
 141     return (::bind(mSocket, (struct sockaddr*)&addr, sizeof(addr)) == 0);
 142 }
 143
 144 bool CANSocketRaw::closeSocket()
 145 {
 146     return (::close(mSocket) == 0);
 147 }
 148
 149 int CANSocketRaw::waitData(unsigned int timeout)
 150 {
 151     return ::poll(&mPoll, 1, timeout);
 152 }
 153
 154 bool CANSocketRaw::writeFrame(const can_frame& frame)
 155 {
 156     return ::write(mSocket, &frame, sizeof(frame)) == sizeof(frame);
 157 }
 158
 159 ssize_t CANSocketRaw::readFrame(can_frame& frame, double &timestamp)
 160 {
 161     struct iovec io;
 162     struct msghdr msgh;
 163     struct cmsghdr *cmsg;
 164
 165     // prepare buffers
 166     memset(&msgh, 0, sizeof(msgh));
 167     io.iov_base=&frame;
 168     io.iov_len=sizeof(can_frame);
 169     msgh.msg_iov=&io;
 170     msgh.msg_iovlen=1;
 171     char buffer[1024];
 172     msgh.msg_control=&buffer;
 173     msgh.msg_controllen=sizeof(buffer);
 174
 175     // receive data
 176     ssize_t nbytes = ::recvmsg(mSocket, &msgh, 0);
 177
 178     if (nbytes > 0 )
 179     {
 180         /* Receive auxiliary data in msgh */
 181         for (cmsg = CMSG_FIRSTHDR(&msgh); cmsg != NULL;
 182              cmsg = CMSG_NXTHDR(&msgh, cmsg)) {
 183             if (cmsg->cmsg_type == SO_TIMESTAMP) {
 184                 struct ::timeval *tv = (struct timeval*) CMSG_DATA(cmsg);
 185
 186                 // convert the timestamp
 187                  timestamp =  amb::Timestamp::fromTimeval(*tv);
 188
 189                 break;
 190             }
 191         }
 192         if (cmsg == NULL) {
 193             /* No timestamp is provided by the socket. Use our own. */
 194             timestamp = amb::Timestamp::instance()->epochTime();
 195         }
 196     }
 197
 198     return nbytes;
 199 }