2 Copyright (C) 2012 Intel Corporation
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.
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.
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
20 #include <sys/ioctl.h>
22 #include <sys/socket.h>
25 #include "cansocket.h"
26 #include "cansocketraw.h"
27 #include "timestamp.h"
29 CANSocketRaw::CANSocketRaw() :
35 bool CANSocketRaw::start(const char* ifName)
41 LOG_ERROR("Socket error");
43 can_err_mask_t errorMask = 0xFFFFFFFF;
44 if(!enableCANErrors(errorMask)) {
45 LOG_ERROR("Socket error");
47 if(!enableTimestamps()) {
48 LOG_ERROR("Socket error");
51 mPoll.events = POLLIN | POLLPRI;
53 memset(&ifr, 0, sizeof(ifr));
54 strcpy(ifr.ifr_name, ifName);
55 if(!locateInterfaceIndex(ifr)) {
56 LOG_ERROR("Socket error");
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");
76 void CANSocketRaw::stop()
86 bool CANSocketRaw::write(const struct CANFrameInfo &message)
90 return writeFrame(message.frame);
93 CANSocket::CANSocketReadSuccess CANSocketRaw::read(struct CANFrameInfo& message, unsigned int timeout)
95 LOG_TRACE("timeout: " << timeout);
97 CANSocket::CANSocketReadSuccess success;
98 memset(&message, 0, sizeof(message));
100 switch(waitData(timeout)) {
102 LOG_ERROR("reading error");
103 success = CANSocket::READING_FAILED;
106 success = CANSocket::READING_TIMED_OUT;
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;
117 bool CANSocketRaw::createSocket()
119 return ((mSocket = ::socket(PF_CAN, SOCK_RAW, CAN_RAW)) >= 0);
122 bool CANSocketRaw::enableTimestamps()
124 const int timestamp = 1;
126 return (setsockopt(mSocket, SOL_SOCKET, SO_TIMESTAMP, ×tamp, sizeof(timestamp)) == 0);
129 bool CANSocketRaw::enableCANErrors(can_err_mask_t errorMask)
131 return (setsockopt(mSocket, SOL_CAN_RAW, CAN_RAW_ERR_FILTER, &errorMask, sizeof(errorMask)) == 0);
134 bool CANSocketRaw::locateInterfaceIndex(struct ifreq& ifr)
136 return (::ioctl(mSocket, SIOCGIFINDEX, &ifr) == 0);
139 bool CANSocketRaw::bindSocket(struct sockaddr_can& addr)
141 return (::bind(mSocket, (struct sockaddr*)&addr, sizeof(addr)) == 0);
144 bool CANSocketRaw::closeSocket()
146 return (::close(mSocket) == 0);
149 int CANSocketRaw::waitData(unsigned int timeout)
151 return ::poll(&mPoll, 1, timeout);
154 bool CANSocketRaw::writeFrame(const can_frame& frame)
156 return ::write(mSocket, &frame, sizeof(frame)) == sizeof(frame);
159 ssize_t CANSocketRaw::readFrame(can_frame& frame, double ×tamp)
163 struct cmsghdr *cmsg;
166 memset(&msgh, 0, sizeof(msgh));
168 io.iov_len=sizeof(can_frame);
172 msgh.msg_control=&buffer;
173 msgh.msg_controllen=sizeof(buffer);
176 ssize_t nbytes = ::recvmsg(mSocket, &msgh, 0);
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);
186 // convert the timestamp
187 timestamp = amb::Timestamp::fromTimeval(*tv);
193 /* No timestamp is provided by the socket. Use our own. */
194 timestamp = amb::Timestamp::instance()->epochTime();