socketcan.h
Go to the documentation of this file.
00001 #ifndef H_SOCKETCAN_DRIVER
00002 #define H_SOCKETCAN_DRIVER
00003 
00004 #include <socketcan_interface/asio_base.h>
00005 #include <boost/bind.hpp>
00006 
00007 #include <sys/types.h>
00008 #include <sys/socket.h>
00009 #include <sys/ioctl.h>
00010 #include <net/if.h>
00011  
00012 #include <linux/can.h>
00013 #include <linux/can/raw.h>
00014 #include <linux/can/error.h>
00015 
00016 #include <socketcan_interface/dispatcher.h>
00017 
00018 namespace can {
00019 
00020 class SocketCANInterface : public AsioDriver<boost::asio::posix::stream_descriptor> {
00021     bool loopback_;
00022     int sc_;
00023 public:    
00024     SocketCANInterface()
00025     : loopback_(false), sc_(-1)
00026     {}
00027     
00028     virtual bool doesLoopBack() const{
00029         return loopback_;
00030     }
00031 
00032     virtual bool init(const std::string &device, bool loopback){
00033         State s = getState();
00034         if(s.driver_state == State::closed){
00035             sc_ = 0;
00036             device_ = device;
00037             loopback_ = loopback;
00038 
00039             int sc = socket( PF_CAN, SOCK_RAW, CAN_RAW );
00040             if(sc < 0){
00041                 setErrorCode(boost::system::error_code(sc,boost::system::system_category()));
00042                 return false;
00043             }
00044             
00045             struct ifreq ifr;
00046             strcpy(ifr.ifr_name, device_.c_str());
00047             int ret = ioctl(sc, SIOCGIFINDEX, &ifr);
00048 
00049             if(ret != 0){
00050                 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
00051                 close(sc);
00052                 return false;
00053             }
00054             can_err_mask_t err_mask =
00055                 ( CAN_ERR_TX_TIMEOUT   /* TX timeout (by netdevice driver) */
00056                 | CAN_ERR_LOSTARB      /* lost arbitration    / data[0]    */
00057                 | CAN_ERR_CRTL         /* controller problems / data[1]    */
00058                 | CAN_ERR_PROT         /* protocol violations / data[2..3] */
00059                 | CAN_ERR_TRX          /* transceiver status  / data[4]    */
00060                 | CAN_ERR_ACK           /* received no ACK on transmission */
00061                 | CAN_ERR_BUSOFF        /* bus off */
00062                 //CAN_ERR_BUSERROR      /* bus error (may flood!) */
00063                 | CAN_ERR_RESTARTED     /* controller restarted */
00064             ); 
00065 
00066             ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_ERR_FILTER,
00067                &err_mask, sizeof(err_mask));
00068             
00069             if(ret != 0){
00070                 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
00071                 close(sc);
00072                 return false;
00073             }
00074             
00075             if(loopback_){
00076                 int recv_own_msgs = 1; /* 0 = disabled (default), 1 = enabled */
00077                 ret = setsockopt(sc, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs));
00078                 
00079                 if(ret != 0){
00080                     setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
00081                     close(sc);
00082                     return false;
00083                 }
00084             }
00085             
00086             struct sockaddr_can addr = {0};
00087             addr.can_family = AF_CAN;
00088             addr.can_ifindex = ifr.ifr_ifindex;
00089             ret = bind( sc, (struct sockaddr*)&addr, sizeof(addr) );            
00090 
00091             if(ret != 0){
00092                 setErrorCode(boost::system::error_code(ret,boost::system::system_category()));
00093                 close(sc);
00094                 return false;
00095             }
00096             
00097             boost::system::error_code ec;
00098             socket_.assign(sc,ec);
00099             
00100             setErrorCode(ec);
00101             
00102             if(ec){
00103                 close(sc);
00104                 return false;
00105             }
00106             setInternalError(0);
00107             setDriverState(State::open);
00108             sc_ = sc;
00109             return true;
00110         }
00111         return getState().isReady();
00112     }
00113     virtual bool recover(){
00114         if(!getState().isReady()){
00115             shutdown();
00116             return init(device_, doesLoopBack());
00117         }
00118         return getState().isReady();
00119     }
00120     virtual bool translateError(unsigned int internal_error, std::string & str){
00121 
00122         bool ret = false;
00123         if(!internal_error){
00124             str = "OK";
00125             ret = true;
00126         }
00127         if( internal_error & CAN_ERR_TX_TIMEOUT){
00128             str += "TX timeout (by netdevice driver);";
00129             ret = true;
00130         }
00131         if( internal_error & CAN_ERR_LOSTARB){
00132             str += "lost arbitration;";
00133             ret = true;
00134         }
00135         if( internal_error & CAN_ERR_CRTL){
00136             str += "controller problems;";
00137             ret = true;
00138         }
00139         if( internal_error & CAN_ERR_PROT){
00140             str += "protocol violations;";
00141             ret = true;
00142         }
00143         if( internal_error & CAN_ERR_TRX){
00144             str += "transceiver status;";
00145             ret = true;
00146         }
00147         if( internal_error & CAN_ERR_BUSOFF){
00148             str += "bus off;";
00149             ret = true;
00150         }
00151         if( internal_error & CAN_ERR_RESTARTED){
00152             str += "ontroller restarted;";
00153             ret = true;
00154         }
00155         return ret;
00156     }
00157     int getInternalSocket() {
00158         return sc_;
00159     }
00160 protected:
00161     std::string device_;
00162     can_frame frame_;
00163     
00164     virtual void triggerReadSome(){
00165         boost::mutex::scoped_lock lock(send_mutex_);
00166         socket_.async_read_some(boost::asio::buffer(&frame_, sizeof(frame_)), boost::bind( &SocketCANInterface::readFrame,this, boost::asio::placeholders::error));
00167     }
00168     
00169     virtual bool enqueue(const Frame & msg){
00170         boost::mutex::scoped_lock lock(send_mutex_); //TODO: timed try lock
00171 
00172         can_frame frame = {0};
00173         frame.can_id = msg.id | (msg.is_extended?CAN_EFF_FLAG:0) | (msg.is_rtr?CAN_RTR_FLAG:0);;
00174         frame.can_dlc = msg.dlc;
00175         
00176         
00177         for(int i=0; i < frame.can_dlc;++i)
00178             frame.data[i] = msg.data[i];
00179         
00180         boost::system::error_code ec;
00181         boost::asio::write(socket_, boost::asio::buffer(&frame, sizeof(frame)),boost::asio::transfer_all(), ec);
00182         if(ec){
00183             LOG("FAILED " << ec);
00184             setErrorCode(ec);
00185             setNotReady();
00186             return false;
00187         }
00188         
00189         return true;
00190     }
00191     
00192     void readFrame(const boost::system::error_code& error){
00193         if(!error){
00194             input_.dlc = frame_.can_dlc;
00195             for(int i=0;i<frame_.can_dlc && i < 8; ++i){
00196                 input_.data[i] = frame_.data[i];
00197             }
00198             
00199             if(frame_.can_id & CAN_ERR_FLAG){ // error message
00200                 input_.id = frame_.can_id & CAN_EFF_MASK;
00201                 input_.is_error = 1;
00202 
00203                 LOG("error: " << input_.id);
00204                 setInternalError(input_.id);
00205                 setNotReady();
00206 
00207             }else{
00208                 input_.is_extended = (frame_.can_id & CAN_EFF_FLAG) ? 1 :0;
00209                 input_.id = frame_.can_id & (input_.is_extended ? CAN_EFF_MASK : CAN_SFF_MASK);
00210                 input_.is_error = 0;
00211                 input_.is_rtr = (frame_.can_id & CAN_RTR_FLAG) ? 1 : 0;
00212             }
00213 
00214         }
00215         frameReceived(error);
00216     }
00217 private:
00218     boost::mutex send_mutex_;
00219 };
00220 
00221 typedef SocketCANInterface SocketCANDriver;
00222 
00223 template <typename T> class ThreadedInterface;
00224 typedef ThreadedInterface<SocketCANInterface> ThreadedSocketCANInterface;
00225 
00226 
00227 } // namespace can
00228 #endif


socketcan_interface
Author(s): Mathias L├╝dtke
autogenerated on Sun Sep 3 2017 03:10:38