plugins/common/cansocketbcm.cpp

   1 /*
   2 Copyright (C) 2012 Intel Corporation
   3 Copyright (C) 2015 Cogent Embedded Inc.
   4 Copyright (C) 2015 Renesas Electronics Corporation
   5
   6 This library is free software; you can redistribute it and/or
   7 modify it under the terms of the GNU Lesser General Public
   8 License as published by the Free Software Foundation; either
   9 version 2.1 of the License, or (at your option) any later version.
  10
  11 This library is distributed in the hope that it will be useful,
  12 but WITHOUT ANY WARRANTY; without even the implied warranty of
  13 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  14 Lesser General Public License for more details.
  15
  16 You should have received a copy of the GNU Lesser General Public
  17 License along with this library; if not, write to the Free Software
  18 Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301  USA
  19 */
  20
  21 #include <string.h>
  22 #include <sys/ioctl.h>
  23 #include <unistd.h>
  24 #include <sys/socket.h>
  25
  26 #include <timestamp.h>
  27 #include "logger.h"
  28 #include "cansocketbcm.h"
  29 #include "timestamp.h"
  30
  31 CANSocketBCM::CANSocketBCM() :
  32     mSocket(-1)
  33 {
  34     LOG_TRACE("");
  35 }
  36
  37 bool CANSocketBCM::start(const char* ifName)
  38 {
  39     LOG_TRACE("");
  40
  41     if(mSocket >= 0)
  42         return false;
  43
  44     if(!createSocket())
  45     {
  46         LOG_ERROR("Socket error");
  47         return false;
  48     }
  49
  50     //    can_err_mask_t errorMask = 0xFFFFFFFF;
  51     //    if(!enableCANErrors(errorMask)) {
  52     //        LOG_ERROR("Socket error");
  53     //        return false;
  54     //    }
  55
  56     mPoll.fd = mSocket;
  57     mPoll.events = POLLIN | POLLPRI;
  58
  59     struct ifreq ifr;
  60     memset(&ifr, 0, sizeof(ifr));
  61     strcpy(ifr.ifr_name, ifName);
  62     if(!locateInterfaceIndex(ifr)) {
  63         LOG_ERROR("Socket error");
  64         stop();
  65         return false;
  66     }
  67
  68     struct sockaddr_can addr;
  69     memset(&addr, 0, sizeof(addr));
  70     addr.can_family = AF_CAN;
  71     addr.can_ifindex = ifr.ifr_ifindex;
  72     if(!connectSocket(addr)) {
  73         LOG_ERROR("Socket error");
  74         stop();
  75         return false;
  76     }
  77
  78     return true;
  79 }
  80
  81 void CANSocketBCM::stop()
  82 {
  83     LOG_TRACE("");
  84
  85     if(mSocket >= 0) {
  86         closeSocket();
  87         mSocket = -1;
  88     }
  89 }
  90
  91 bool CANSocketBCM::write(const struct CANFrameInfo &message)
  92 {
  93     LOG_TRACE("");
  94
  95     return writeFrameOneTime(message.frame);
  96 }
  97
  98 CANSocket::CANSocketReadSuccess CANSocketBCM::read(struct CANFrameInfo& message, unsigned int timeout)
  99 {
 100     LOG_TRACE("timeout: " << timeout);
 101
 102     CANSocket::CANSocketReadSuccess success;
 103     memset(&message, 0, sizeof(message));
 104
 105     switch(waitData(timeout)) {
 106     case -1:
 107         LOG_ERROR("reading error");
 108         success = CANSocket::READING_FAILED;
 109         break;
 110     case 0:
 111         success = CANSocket::READING_TIMED_OUT;
 112         break;
 113     default:
 114         success = readMessage(message);
 115         break;
 116     }
 117
 118     return success;
 119 }
 120
 121 bool CANSocketBCM::createSocket()
 122 {
 123     return ((mSocket = ::socket(PF_CAN, SOCK_DGRAM, CAN_BCM)) >= 0);
 124 }
 125
 126 bool CANSocketBCM::locateInterfaceIndex(struct ifreq& ifr)
 127 {
 128     return (::ioctl(mSocket, SIOCGIFINDEX, &ifr) == 0);
 129 }
 130
 131 bool CANSocketBCM::connectSocket(struct sockaddr_can& addr)
 132 {
 133     return (::connect(mSocket, (struct sockaddr*)&addr, sizeof(addr)) == 0);
 134 }
 135
 136 bool CANSocketBCM::closeSocket()
 137 {
 138     return (::close(mSocket) == 0);
 139 }
 140
 141 int CANSocketBCM::waitData(unsigned int timeout)
 142 {
 143     return ::poll(&mPoll, 1, timeout);
 144 }
 145
 146 /**
 147  * BCM header with one message.
 148  * @note hdr.nframes must always be 0 or 1.
 149  */
 150 struct __attribute__ ((__packed__)) bcm_msg_one{
 151     struct bcm_msg_head hdr;
 152     struct can_frame frames[1];
 153 };
 154
 155 bool CANSocketBCM::writeFrameOneTime(const can_frame& frame)
 156 {
 157     struct bcm_msg_one bcms;
 158
 159     // fill in the header
 160     memset(&bcms.hdr, 0, sizeof(bcms.hdr));
 161     bcms.hdr.opcode = TX_SEND;
 162     bcms.hdr.nframes = 1;
 163     bcms.hdr.can_id = frame.can_id;
 164
 165     // copy the frame
 166     memcpy(&bcms.frames[0], &frame, sizeof(frame));
 167
 168     // and write everything
 169     ssize_t nbytes = ::write(mSocket, &bcms, sizeof(bcms));
 170     return nbytes == sizeof(bcms);
 171 }
 172
 173 CANSocket::CANSocketReadSuccess CANSocketBCM::readMessage(CANFrameInfo& message)
 174 {
 175     struct bcm_msg_one bcms;
 176
 177     // clear the destination
 178     memset(&message, 0, sizeof(message));
 179
 180     // get data from socket
 181     size_t nbytes = ::recv(mSocket, &bcms, sizeof(bcms), 0);
 182     if ( nbytes < sizeof(bcms.hdr))
 183     {
 184         LOG_ERROR("Socket error");
 185         return CANSocket::CANSocketReadSuccess::READING_FAILED;
 186     }
 187     //TODO: implement better timestamps
 188     message.timestamp = amb::currentTime();
 189
 190     switch (bcms.hdr.opcode)
 191     {
 192     case RX_CHANGED:
 193         if (bcms.hdr.nframes >= 1 && nbytes == sizeof(bcms))
 194         {
 195             if (bcms.hdr.nframes > 1)
 196             {
 197                 LOG_WARNING("Dropped " << bcms.hdr.nframes - 1 << " updates from CAN bus.");
 198             }
 199
 200             // copy the first frame
 201             memcpy(&message.frame, &bcms.frames[0], sizeof(bcms.frames[0]));
 202             message.status = CANFrameInfo::CANMessageStatus::GOOD;
 203             return CANSocket::CANSocketReadSuccess::READING_SUCCEEDED;
 204         }
 205         else
 206         {
 207             LOG_ERROR("Unexpected data from the socket"
 208                       << " " << bcms.hdr.opcode
 209                       << " " << bcms.hdr.nframes
 210                       << " " << nbytes);
 211             return CANSocket::CANSocketReadSuccess::READING_FAILED;
 212         }
 213     case RX_TIMEOUT:
 214         memcpy(&message.frame, &bcms.frames[0], sizeof(bcms.frames[0]));
 215         message.frame.can_id = bcms.hdr.can_id; //doubtful. Do we need to override this?
 216         message.status = CANFrameInfo::CANMessageStatus::TIMEOUT;
 217         return CANSocket::CANSocketReadSuccess::READING_SUCCEEDED;
 218
 219     case TX_EXPIRED:
 220         // do nothing
 221         return CANSocket::CANSocketReadSuccess::READING_TIMED_OUT;
 222
 223     default:
 224         LOG_ERROR("Unexpected opcode " << bcms.hdr.opcode);
 225         return CANSocket::CANSocketReadSuccess::READING_FAILED;
 226     }
 227 }
 228
 229 /*
 230  4.2.5 Broadcast Manager receive filter timers
 231
 232    The timer values ival1 or ival2 may be set to non-zero values at RX_SETUP.
 233    When the SET_TIMER flag is set the timers are enabled:
 234
 235    ival1: Send RX_TIMEOUT when a received message is not received again within
 236     the given time. When START_TIMER is set at RX_SETUP the timeout detection
 237      is activated directly - even without a former CAN frame reception.
 238
 239    ival2: Throttle the received message rate down to the value of ival2. This
 240      is useful to reduce messages for the application when the signal inside the
 241      CAN frame is stateless as state changes within the ival2 periode may get
 242      lost.
 243 */
 244
 245 bool CANSocketBCM::registerCyclicMessageForReceive(int canId, double minCycleTime, double maxCycleTime)
 246 {
 247     struct bcm_msg_head hdr;
 248
 249     // fill in the header
 250     memset(&hdr, 0, sizeof(hdr));
 251     hdr.opcode = RX_SETUP;
 252     // set RX_FILTER_ID | RX_CHECK_DLC because we don't differentiate messages by dlc or content yet. Only by id
 253     // setting RX_ANNOUNCE_RESUME may lead to duplicates in data which should be filtered by amb core.
 254     //        However, we won't miss any data.
 255     hdr.flags = RX_FILTER_ID | RX_CHECK_DLC | SETTIMER | STARTTIMER | RX_ANNOUNCE_RESUME;
 256     hdr.nframes = 0;
 257     hdr.can_id = canId;
 258     hdr.ival1 = amb::Timestamp::toTimeval(maxCycleTime);
 259     hdr.ival2 = amb::Timestamp::toTimeval(minCycleTime);
 260
 261     // and write
 262     ssize_t nbytes = ::write(mSocket, &hdr, sizeof(hdr));
 263     return nbytes == sizeof(hdr);
 264 }
 265
 266 bool CANSocketBCM::unregisterMessageForReceive(int canId)
 267 {
 268     struct bcm_msg_head hdr;
 269
 270     // fill in the header
 271     memset(&hdr, 0, sizeof(hdr));
 272     hdr.opcode = RX_DELETE;
 273     hdr.can_id = canId;
 274
 275     // and write
 276     ssize_t nbytes = ::write(mSocket, &hdr, sizeof(hdr));
 277     return nbytes == sizeof(hdr);
 278 }
 279