socketcan.cpp
Go to the documentation of this file.
00001 /*
00002         Aseba - an event-based framework for distributed robot control
00003         Copyright (C) 2012:
00004                 Philippe Retornaz <philippe.retornaz@epfl.ch>
00005                 and other contributors, see authors.txt for details
00006         
00007         This program is free software: you can redistribute it and/or modify
00008         it under the terms of the GNU Lesser General Public License as published
00009         by the Free Software Foundation, version 3 of the License.
00010         
00011         This program is distributed in the hope that it will be useful,
00012         but WITHOUT ANY WARRANTY; without even the implied warranty of
00013         MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014         GNU Lesser General Public License for more details.
00015         
00016         You should have received a copy of the GNU Lesser General Public License
00017         along with this program. If not, see <http://www.gnu.org/licenses/>.
00018 */
00019 
00020 
00021 #include <sys/ioctl.h>
00022 #include <sys/uio.h>
00023 #include <net/if.h>
00024 
00025 #include <sys/socket.h>
00026 #include <linux/can.h>
00027 #include <linux/can/raw.h>
00028 
00029 #include <unistd.h>
00030 
00031 #include <string>
00032 #include <iostream>
00033 
00034 #include <cstring>
00035 
00036 #include <dashel/dashel.h>
00037 #include <dashel/dashel-posix.h>
00038 
00039 #include "../../common/consts.h"
00040 
00041 #ifndef AF_CAN
00042 #define AF_CAN          29
00043 #endif
00044 #ifndef PF_CAN
00045 #define PF_CAN          AF_CAN
00046 #endif
00047 
00048 #ifndef SO_RXQ_OVFL
00049 #define SO_RXQ_OVFL     40
00050 #endif
00051 
00052 using namespace Dashel;
00053 using namespace std;
00054 
00055 
00056 /* CanStream implementation
00057  * This is the protocol used in the MarxBot and Handbot robots.
00058  * It needs a SocketCAN interface (Linux native CAN interfaces).
00059  *
00060  * This stream (named "can") accept only one parameter:
00061  *      if : The CAN interface to use
00062  *
00063  * The interface must already be configured & upped by your distribution script.
00064  *
00065  * Usage example:
00066  *      "can:if=can0"
00067  *
00068  */
00069 
00070 class CanStream: public SelectableStream
00071 {
00072 #define TYPE_SMALL_PACKET 0x3
00073 #define TYPE_PACKET_NORMAL 0x0
00074 #define TYPE_PACKET_START 0x1
00075 #define TYPE_PACKET_STOP 0x2
00076 
00077 #define CANID_TO_TYPE(canid) ((canid) >> 8)
00078 #define CANID_TO_ID(canid) ((int) ((canid) & 0xFF))
00079 #define TO_CANID(type,id) (((type) << 8) | (id))
00080 
00081         protected:
00082                 unsigned char tx_buffer[ASEBA_MAX_OUTER_PACKET_SIZE];
00083                 int tx_len;
00084                 unsigned char rx_buffer[ASEBA_MAX_OUTER_PACKET_SIZE];
00085                 unsigned int rx_len;
00086                 unsigned int rx_p;
00087 
00088                 struct iovec iov;
00089                 struct msghdr msg;
00090                 struct sockaddr_can addr;
00091                 char ctrlmsg[CMSG_SPACE(sizeof(struct timeval)) + CMSG_SPACE(sizeof(__u32))];
00092                 struct can_frame rframe;
00093 
00094 #define RX_CAN_SIZE 1000
00095                 struct {
00096                         struct can_frame f;
00097                         int used;
00098                 } rx_fifo[RX_CAN_SIZE];
00099                 int rx_insert;
00100                 int rx_consume;
00101         public:
00102                 CanStream(const string &targetName) :
00103                         Stream("can"),
00104                         SelectableStream("can")
00105                 {
00106                         struct ifreq ifr;
00107 
00108                         target.add("can:if=can0");
00109                         target.add(targetName.c_str());
00110                         string ifName;
00111                         if(target.isSet("if"))
00112                         {
00113                                 target.addParam("if", NULL, true);
00114                                 ifName = target.get("if");
00115                         }
00116                         fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
00117                         if(fd < 0)
00118                                 throw DashelException(DashelException::ConnectionFailed, 0, "Socket creation failed");
00119                         
00120                         addr.can_family = AF_CAN;
00121                         if(strlen(ifName.c_str()) >= IFNAMSIZ)
00122                                 throw DashelException(DashelException::ConnectionFailed, 0, "Interface name too long");
00123                         
00124                         strcpy(ifr.ifr_name, ifName.c_str());
00125                         if(ioctl(fd, SIOCGIFINDEX, &ifr) < 0)
00126                                 throw DashelException(DashelException::ConnectionFailed, 0, "Unable to get interface");
00127                         
00128                         // Try to have 2Mb RX buffer
00129                         int options = 2*1024*1024;
00130                         setsockopt(fd, SOL_SOCKET, SO_RCVBUFFORCE, &options, sizeof(options));
00131                         
00132                         // Enable monitoring of dropped packets
00133                         options = 1;
00134                         setsockopt(fd, SOL_SOCKET, SO_RXQ_OVFL, &options, sizeof(options));
00135 
00136                         addr.can_ifindex = ifr.ifr_ifindex;
00137                         if(bind(fd, (struct sockaddr *) &addr, sizeof(addr)) < 0)
00138                                 throw DashelException(DashelException::ConnectionFailed, 0, "Unable to bind");
00139 
00140                         rx_insert = rx_consume = 0;
00141                         tx_len = 0;
00142                         rx_len = 0;
00143                         rx_p = 0;
00144                         memset(rx_fifo, 0, sizeof(rx_fifo));
00145                         iov.iov_base = &rframe;
00146                         msg.msg_iov = &iov;
00147                         msg.msg_name = &addr;
00148                         msg.msg_iovlen = 1;
00149                         msg.msg_control = ctrlmsg;
00150                 }
00151         private:
00152                 int is_packet_tx(void)
00153                 {
00154                         int packet_len;
00155                         if(tx_len < 6)
00156                                 return 0;
00157                         packet_len = tx_buffer[0] | (tx_buffer[1] << 8); // Little endian
00158                         if(tx_len >= packet_len + 6)
00159                                 return 1;
00160                         return 0;
00161                 }
00162 
00163                 void send_aseba_packet()
00164                 {
00165                         struct can_frame frame;
00166                         unsigned int packet_len = tx_buffer[0] | (tx_buffer[1] << 8); // Little endian;
00167                         unsigned int nodeId = tx_buffer[2] | (tx_buffer[3] << 8);
00168                         unsigned int msgId = tx_buffer[4] | (tx_buffer[5] << 8);
00169                         
00170                         if(packet_len  <= 6) 
00171                         {
00172                                 // Small packet
00173                                 frame.can_id = TO_CANID(TYPE_SMALL_PACKET, nodeId);
00174                                 frame.can_dlc = packet_len + 2;
00175                                 frame.data[0] = msgId;
00176                                 frame.data[1] = msgId >> 8;
00177                                 memcpy(&frame.data[2], &tx_buffer[6], packet_len);
00178                                 if(::write(fd,&frame, sizeof(frame)) != sizeof(frame))
00179                                         throw DashelException(DashelException::IOError, 0, "Write error");
00180                         } 
00181                         else 
00182                         {
00183                                 unsigned char * p = &tx_buffer[6];
00184                                 frame.can_id = TO_CANID(TYPE_PACKET_START, nodeId);
00185                                 frame.can_dlc = 8;
00186                                 frame.data[0] = msgId;
00187                                 frame.data[1] = msgId >> 8;
00188                                 memcpy(&frame.data[2], p, 6);
00189                                 p += 6;
00190                                 packet_len -= 6;
00191                                 if(::write(fd, &frame, sizeof(frame)) != sizeof(frame)) 
00192                                         throw DashelException(DashelException::IOError, 0, "Write error");
00193 
00194                                 while(packet_len > 8)
00195                                 {
00196                                         frame.can_id = TO_CANID(TYPE_PACKET_NORMAL, nodeId);
00197                                         frame.can_dlc = 8;
00198                                         memcpy(frame.data, p, 8);
00199                                         p+=8;
00200                                         packet_len -= 8;
00201                                         if(::write(fd, &frame, sizeof(frame)) != sizeof(frame))
00202                                                 throw DashelException(DashelException::IOError, 0, "Write error");
00203                                 }
00204                                 frame.can_id = TO_CANID(TYPE_PACKET_STOP, nodeId);
00205                                 frame.can_dlc = packet_len;
00206                                 memcpy(frame.data, p, packet_len);
00207                                 if(::write(fd, &frame, sizeof(frame)) != sizeof(frame))
00208                                         throw DashelException(DashelException::IOError, 0, "Write error");
00209                         }
00210                         tx_len = 0;
00211                 }
00212         public:
00213                 virtual void write(const void *data, const size_t size)
00214                 {
00215                         size_t s = size;
00216                         const unsigned char * d = (const unsigned char * ) data;
00217                         while(s--)
00218                         {
00219                                 tx_buffer[tx_len++] = *d++;
00220                                 if(is_packet_tx())
00221                                         send_aseba_packet();
00222                         }
00223                 }
00224 
00225                 virtual void flush() 
00226                 {
00227                 }
00228         private:
00229                 void pack_fifo()
00230                 {
00231                         while(!rx_fifo[rx_consume].used && rx_consume != rx_insert)
00232                         {
00233                                 if(++rx_consume == RX_CAN_SIZE)
00234                                         rx_consume = 0;
00235                         }
00236                 }
00237 
00238                 // 1 if defragment was sucessfull.
00239                 // 0 if not,
00240                 // -1 if needs to recall (one packet was dropped because not full)
00241                 int defragment(void)
00242                 {
00243                         int i;
00244                         int stopId;
00245                         int stopPos = -1;
00246                         int ignore = 0;
00247                         for(i = rx_consume; i != rx_insert; ) 
00248                         {
00249                                 if(rx_fifo[i].used)
00250                                 {
00251                                         if(CANID_TO_TYPE(rx_fifo[i].f.can_id) == TYPE_SMALL_PACKET)
00252                                         {
00253                                                 if(rx_fifo[i].f.can_dlc < 2)
00254                                                         throw DashelException(DashelException::IOError, 0, "Packet too short");
00255 
00256                                                 rx_buffer[0] = rx_fifo[i].f.can_dlc - 2;
00257                                                 rx_buffer[1] = 0;
00258                                                 rx_buffer[2] = CANID_TO_ID(rx_fifo[i].f.can_id);
00259                                                 rx_buffer[3] = 0;
00260                                                 memcpy(&rx_buffer[4], rx_fifo[i].f.data, rx_fifo[i].f.can_dlc);
00261                                                 rx_p = 0;
00262                                                 rx_len = rx_fifo[i].f.can_dlc + 4;
00263                                                  
00264                                                 rx_fifo[i].used = 0; // Free the frame (pack_fifo())
00265                                                 return 1;
00266                                         }
00267                                         if(CANID_TO_TYPE(rx_fifo[i].f.can_id) == TYPE_PACKET_STOP)
00268                                         {
00269                                                 stopPos = i;
00270                                                 stopId = CANID_TO_ID(rx_fifo[i].f.can_id);
00271                                                 break;
00272                                         }
00273                                 }
00274                                 i++;
00275                                 if(i == RX_CAN_SIZE)
00276                                         i = 0;
00277                         }
00278                         if(stopPos < 0)
00279                                 return 0;
00280                                 
00281                         i = rx_consume;
00282                         // Len will be filled lated
00283                         rx_buffer[2] = stopId;
00284                         rx_buffer[3] = 0;
00285                         rx_len = 4;
00286                         while(1) 
00287                         {
00288                                 if(rx_fifo[i].used && CANID_TO_ID(rx_fifo[i].f.can_id) == stopId)
00289                                 {
00290                                         if(rx_len == 4 && CANID_TO_TYPE(rx_fifo[i].f.can_id) != TYPE_PACKET_START)
00291                                                 // We got a stop, but not a start, let's ignore this packet
00292                                                 ignore = 1;
00293 
00294                                         if(rx_len + rx_fifo[i].f.can_dlc > sizeof(rx_buffer))
00295                                                 throw DashelException(DashelException::IOError, 0, "Packet too large!");
00296 
00297                                         memcpy(&rx_buffer[rx_len], rx_fifo[i].f.data, rx_fifo[i].f.can_dlc);
00298                                         rx_len += rx_fifo[i].f.can_dlc;
00299                                         rx_fifo[i].used = 0;    
00300 
00301                                         if(i == stopPos)
00302                                                 break;
00303                                 }
00304                                 if(++i == RX_CAN_SIZE)
00305                                         i = 0;
00306                         }
00307                         if(ignore)
00308                         {
00309                                 rx_len = 0;
00310                                 rx_p = 0;
00311                                 return -1;
00312                         } 
00313                         else
00314                         {
00315                                 rx_buffer[0] = rx_len - 6;
00316                                 rx_buffer[1] = (rx_len - 6) >> 8;
00317                                 rx_p = 0;
00318                                 return 1;
00319                         }
00320                 }
00321                 int fifo_full() 
00322                 {
00323                         int i = rx_insert + 1;
00324                         if(i == RX_CAN_SIZE)
00325                                 i = 0;
00326                         return i == rx_consume;
00327                 }
00328 
00329                 void read_iface(void)
00330                 {
00331                         int def;
00332                         while(1)
00333                         {
00334                                 while((def = defragment()) == -1);
00335                                 if(def == 1)
00336                                         break;
00337 
00338                                 struct cmsghdr *cmsg;
00339                                 iov.iov_len = sizeof(rframe);
00340                                 msg.msg_namelen = sizeof(addr);
00341                                 msg.msg_controllen = sizeof(ctrlmsg);
00342                                 msg.msg_flags = 0;
00343                                 if(recvmsg(fd, &msg, 0) < (int) sizeof(rframe))
00344                                         throw DashelException(DashelException::IOError, 0, "Read error");
00345 
00346                                 for(cmsg = CMSG_FIRSTHDR(&msg); 
00347                                         cmsg && (cmsg->cmsg_level == SOL_SOCKET);
00348                                         cmsg = CMSG_NXTHDR(&msg,cmsg))
00349                                 {
00350                                         if(cmsg->cmsg_type == SO_RXQ_OVFL)
00351                                         {
00352                                                 __u32 * dropcnt = (__u32 *) CMSG_DATA(cmsg);
00353                                                 if(*dropcnt)
00354                                                         throw DashelException(DashelException::IOError, 0, "Packet dropped");
00355                                         }
00356                                 }
00357                         
00358                                 if(fifo_full())
00359                                         throw DashelException(DashelException::IOError, 0, "Fifo full");
00360 
00361                                 // push to fifo ...
00362                                 memcpy(&rx_fifo[rx_insert].f,&rframe,sizeof(rframe));
00363                                 rx_fifo[rx_insert++].used = 1;
00364                                 if(rx_insert == RX_CAN_SIZE)
00365                                         rx_insert = 0;
00366                         }
00367                         pack_fifo();
00368                 }
00369         public:
00370                 virtual void read(void *data, size_t size) 
00371                 {
00372                         unsigned char * d = (unsigned char *) data;
00373                         while(size)
00374                         {
00375                                 if(rx_len)
00376                                 {
00377                                         *d++ = rx_buffer[rx_p++];
00378                                         rx_len--;
00379                                         size--;
00380                                 } 
00381                                 else 
00382                                 {
00383                                         read_iface();
00384                                 }
00385                         }
00386                 }
00387 
00388                 virtual bool receiveDataAndCheckDisconnection()
00389                 {
00390                         return false;
00391                 }
00392 
00393                 virtual bool isDataInRecvBuffer() const 
00394                 {
00395                         struct can_frame f;
00396                         if(rx_len || rx_insert != rx_consume)
00397                                 return true;
00398 
00399                         if(recv(fd, &f, sizeof(f), MSG_DONTWAIT | MSG_PEEK) == sizeof(f))
00400                                 return true;
00401 
00402                         return false;
00403                 }
00404 
00405 
00406 };
00407 
00408 namespace Dashel
00409 {
00410         void initPlugins()
00411         {
00412                 Dashel::streamTypeRegistry.reg("can", &createInstance<CanStream>);
00413         }
00414 }


aseba
Author(s): Stéphane Magnenat
autogenerated on Thu Jan 2 2014 11:17:17