implemented BCMsocket

socketcan_interface/CMakeLists.txt

@@ -38,12 +38,24 @@ add_library(socketcan_interface_string
 add_executable(socketcan_dump
   src/candump.cpp
 )
+
 target_link_libraries(socketcan_dump
    socketcan_interface_string
    ${catkin_LIBRARIES}
    ${Boost_LIBRARIES}
 )
 
+# socketcan_bcm
+add_executable(socketcan_bcm
+  src/canbcm.cpp
+)
+
+target_link_libraries(socketcan_bcm
+   socketcan_interface_string
+   ${catkin_LIBRARIES}
+   ${Boost_LIBRARIES}
+)
+
 # socketcan_interface_plugin
 add_library(socketcan_interface_plugin
   src/socketcan_interface_plugin.cpp
@@ -55,6 +67,7 @@ target_link_libraries(socketcan_interface_plugin
 
 install(
   TARGETS
+    socketcan_bcm
     socketcan_dump
     socketcan_interface_plugin
     socketcan_interface_string

socketcan_interface/include/socketcan_interface/bcm.h

@@ -0,0 +1,116 @@
+#ifndef H_CAN_BCM
+#define H_CAN_BCM
+
+#include <socketcan_interface/interface.h>
+
+#include <sys/types.h>
+#include <sys/socket.h>
+#include <sys/ioctl.h>
+#include <net/if.h>
+
+#include <linux/can.h>
+#include <linux/can/bcm.h>
+#include <linux/can/error.h>
+
+namespace can {
+
+class BCMsocket{
+    int s_;
+    struct Message {
+        size_t size;
+        uint8_t *data;
+        Message(size_t n)
+        : size(sizeof(bcm_msg_head) + sizeof(can_frame)*n), data(new uint8_t[size])
+        {
+            assert(n<=256);
+            memset(data, 0, size);
+            head().nframes = n;
+        }
+        bcm_msg_head& head() {
+            return *(bcm_msg_head*)data;
+        }
+        template<typename T> void setIVal2(T period){
+            long long usec = boost::chrono::duration_cast<boost::chrono::microseconds>(period).count();
+            head().ival2.tv_sec = usec / 1000000;
+            head().ival2.tv_usec = usec % 1000000;
+        }
+        void setHeader(Header header){
+            head().can_id = header.id | (header.is_extended?CAN_EFF_FLAG:0);
+        }
+        bool write(int s){
+            return ::write(s, data, size) > 0;
+        }
+        ~Message(){
+            delete[] data;
+            data = 0;
+            size = 0;
+        }
+    };
+public:
+    BCMsocket():s_(-1){
+    }
+    bool init(const std::string &device){
+        s_ = socket(PF_CAN, SOCK_DGRAM, CAN_BCM);
+
+        if(s_ < 0 ) return false;
+        struct ifreq ifr;
+        strcpy(ifr.ifr_name, device.c_str());
+        int ret = ioctl(s_, SIOCGIFINDEX, &ifr);
+
+        if(ret != 0){
+            shutdown();
+            return false;
+        }
+
+        struct sockaddr_can addr = {0};
+        addr.can_family = AF_CAN;
+        addr.can_ifindex = ifr.ifr_ifindex;
+
+        ret = connect(s_, (struct sockaddr *)&addr, sizeof(addr));
+
+        if(ret < 0){
+            shutdown();
+            return false;
+        }
+        return true;
+    }
+    template<typename DurationType> bool startTX(DurationType period, Header header, size_t num, Frame *frames) {
+        Message msg(num);
+        msg.setHeader(header);
+        msg.setIVal2(period);
+
+        bcm_msg_head &head = msg.head();
+
+        head.opcode = TX_SETUP;
+        head.flags |= SETTIMER | STARTTIMER;
+
+        for(size_t i=0; i < num; ++i){ // msg nr
+            head.frames[i].can_dlc = frames[i].dlc;
+            head.frames[i].can_id = head.can_id;
+            for(size_t j = 0; j < head.frames[i].can_dlc; ++j){ // byte nr
+                head.frames[i].data[j] = frames[i].data[j];
+            }
+        }
+        return msg.write(s_);
+    }
+    bool stopTX(Header header){
+        Message msg(0);
+        msg.head().opcode = TX_DELETE;
+        msg.setHeader(header);
+        return msg.write(s_);
+    }
+    void shutdown(){
+        if(s_ > 0){
+            close(s_);
+            s_ = -1;
+        }
+    }
+
+    virtual ~BCMsocket(){
+        shutdown();
+    }
+};
+
+}
+
+#endif

socketcan_interface/src/canbcm.cpp

@@ -0,0 +1,39 @@
+#include <socketcan_interface/bcm.h>
+#include <socketcan_interface/string.h>
+
+using namespace can;
+
+#include <iostream>
+
+int main(int argc, char *argv[]){
+    BCMsocket bcm;
+
+    int extra_frames = argc - 4; 
+
+    if(extra_frames < 0){
+        std::cout << "usage: "<< argv[0] << " DEVICE PERIOD HEADER#DATA [DATA*]" << std::endl;
+        return 1;
+    }
+
+    if(!bcm.init(argv[1])){
+        return 2;
+    }
+
+    int num_frames = extra_frames+1;
+    Frame *frames = new Frame[num_frames];
+    Header header = frames[0] = toframe(argv[3]);
+
+    if(extra_frames > 0){
+        for(int i=0; i< extra_frames; ++i){
+            frames[1+i] = toframe(tostring(header,true)+"#"+argv[4+i]);
+        }
+    }
+    for(int i = 0; i < num_frames; ++i){
+        std::cout << frames[i] << std::endl;
+    }
+    if(bcm.startTX(boost::chrono::duration<double>(atof(argv[2])), header, num_frames, frames)){
+        pause();
+        return 0;
+    }
+    return 4;
+}