update

src/test/socketcan.cpp

@@ -1,8 +1,23 @@
 #include "socketcan.hpp"
 
+#include <linux/can.h>
+#include <linux/can/raw.h>
+
 #include "iflytopcpp/core/components/stringutils.hpp"
 //
 using namespace iflytop;
+#define CANID_DELIM     '#'
+#define CC_DLC_DELIM    '_'
+#define DATA_SEPERATOR  '.'
+#define CAN_MAX_DLC     8
+#define CAN_MAX_RAW_DLC 15
+#define CAN_MAX_DLEN    8
+unsigned char asc2nibble(char c) {
+  if ((c >= '0') && (c <= '9')) return c - '0';
+  if ((c >= 'A') && (c <= 'F')) return c - 'A' + 10;
+  if ((c >= 'a') && (c <= 'f')) return c - 'a' + 10;
+  return 16; /* error */
+}
 
 /*******************************************************************************
  *                                 can_frame_t                                 *
@@ -30,20 +45,20 @@ shared_ptr<CanFrame> CanFrame::createStdDataFrame(uint32_t id, uint8_t *data, si
   return canframe;
 }
 
-shared_ptr<CanFrame> CanFrame::createExtRemoteFrame(uint32_t id, uint8_t dlc) {
+shared_ptr<CanFrame> CanFrame::createExtRemoteFrame(uint32_t id) {
   shared_ptr<CanFrame> canframe = make_shared<CanFrame>();
   canframe->m_id                = id;
   canframe->m_canIdentifier     = kextFrame;
   canframe->m_canFrameType      = kremoteframe;
-  canframe->m_dlc               = dlc;
+  canframe->m_dlc               = 0;
   return canframe;
 }
-shared_ptr<CanFrame> CanFrame::createStdRemoteFrame(uint32_t id, uint8_t dlc) {
+shared_ptr<CanFrame> CanFrame::createStdRemoteFrame(uint32_t id) {
   shared_ptr<CanFrame> canframe = make_shared<CanFrame>();
   canframe->m_id                = id;
   canframe->m_canIdentifier     = kstdFrame;
   canframe->m_canFrameType      = kremoteframe;
-  canframe->m_dlc               = dlc;
+  canframe->m_dlc               = 0;
   return canframe;
 }
 void CanFrame::setCanIdentifier(can_identifier_t canIdentifier) { this->m_canIdentifier = canIdentifier; }
@@ -54,26 +69,6 @@ uint32_t         CanFrame::getId() { return m_id; }
 can_identifier_t CanFrame::getCanIdentifier() { return m_canIdentifier; }
 can_frame_type_t CanFrame::getFanFrameType() { return m_canFrameType; }
 
-can_frame_t CanFrame::getCanFrame() {
-  can_frame_t frame;
-  memset(&frame, 0, sizeof(frame));
-
-  if (m_canIdentifier == kstdFrame) {
-    frame.can_id = (m_id & CAN_SFF_MASK);
-  } else {
-    frame.can_id = (m_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
-  }
-
-  if (m_canFrameType == kremoteframe) {
-    frame.can_id |= CAN_RTR_FLAG;
-    frame.can_dlc = m_dlc;
-  } else {
-    frame.can_dlc = m_dlc;
-    memcpy(frame.data, m_data, m_dlc);
-  }
-  return frame;
-}
-
 /*******************************************************************************
  *                                  SocketCan                                  *
  *******************************************************************************/
@@ -177,10 +172,10 @@ int SocketCan::sendFrame(shared_ptr<CanFrame> frame) {
     return -1;
   }
 
-  can_frame_t canframe = frame->getCanFrame();
+  canfd_frame_t canframe = frame->getCanFrame();
 
-  int ret = write(m_canfd, &canframe, sizeof(canframe));
-  if (ret != sizeof(canframe)) {
+  int ret = write(m_canfd, &canframe, canframe.len + 8);
+  if (ret != (canframe.len + 8)) {
     logger->error("write fail,{}", strerror(errno));
     return -1;
   }
@@ -201,7 +196,7 @@ void SocketCan::startListen() {
                    FD_SET(m_canfd, &readFileDescriptors);
 
                    while (!thisThread.getExitFlag()) {
-                     can_frame_t canframe;
+                     canfd_frame_t canframe;
 
                      logger->info("select....");
                      size_t n = select(m_canfd + 1, &readFileDescriptors, 0, 0, &waitTime);
@@ -220,6 +215,145 @@ void SocketCan::startListen() {
                    }
                  }));
 }
+canfd_frame_t CanFrame::getCanFrame() {
+  canfd_frame_t frame;
+#if 1
+  memset(&frame, 0, sizeof(frame));
+
+  if (m_canIdentifier == kstdFrame) {
+    frame.can_id = (m_id & CAN_SFF_MASK);
+  } else {
+    frame.can_id = (m_id & CAN_EFF_MASK) | CAN_EFF_FLAG;
+  }
+
+  if (m_canFrameType == kremoteframe) {
+    frame.can_id |= CAN_RTR_FLAG;
+    frame.len = m_dlc;
+  } else {
+    frame.len = m_dlc;
+    memcpy(frame.data, m_data, m_dlc);
+  }
+#endif
+  return frame;
+}
+
+int SocketCan::parse_canframe(char *cs, struct canfd_frame *cf) {
+  /* documentation see lib.h */
+
+  int     i, idx, dlen, len;
+  int     maxdlen = CAN_MAX_DLEN;
+  int     ret     = CAN_MTU;
+  canid_t tmp;
+
+  len = strlen(cs);
+  // printf("'%s' len %d\n", cs, len);
+
+  memset(cf, 0, sizeof(*cf)); /* init CAN FD frame, e.g. LEN = 0 */
+
+  if (len < 4) return 0;
+
+  if (cs[3] == CANID_DELIM) { /* 3 digits */
+
+    idx = 4;
+    for (i = 0; i < 3; i++) {
+      if ((tmp = asc2nibble(cs[i])) > 0x0F) return 0;
+      cf->can_id |= (tmp << (2 - i) * 4);
+    }
+
+  } else if (cs[8] == CANID_DELIM) { /* 8 digits */
+
+    idx = 9;
+    for (i = 0; i < 8; i++) {
+      if ((tmp = asc2nibble(cs[i])) > 0x0F) return 0;
+      cf->can_id |= (tmp << (7 - i) * 4);
+    }
+    if (!(cf->can_id & CAN_ERR_FLAG)) /* 8 digits but no errorframe?  */
+      cf->can_id |= CAN_EFF_FLAG;     /* then it is an extended frame */
+
+  } else
+    return 0;
+
+  if ((cs[idx] == 'R') || (cs[idx] == 'r')) { /* RTR frame */
+    cf->can_id |= CAN_RTR_FLAG;
+
+    /* check for optional DLC value for CAN 2.0B frames */
+    if (cs[++idx] && (tmp = asc2nibble(cs[idx++])) <= CAN_MAX_DLEN) {
+      cf->len = tmp;
+
+      /* check for optional raw DLC value for CAN 2.0B frames */
+      if ((tmp == CAN_MAX_DLEN) && (cs[idx++] == CC_DLC_DELIM)) {
+        tmp = asc2nibble(cs[idx]);
+        if ((tmp > CAN_MAX_DLEN) && (tmp <= CAN_MAX_RAW_DLC)) {
+          struct can_frame *ccf = (struct can_frame *)cf;
+
+          ccf->__res1 = tmp;
+        }
+      }
+    }
+    return ret;
+  }
+
+  if (cs[idx] == CANID_DELIM) { /* CAN FD frame escape char '##' */
+
+    maxdlen = CANFD_MAX_DLEN;
+    ret     = CANFD_MTU;
+
+    /* CAN FD frame <canid>##<flags><data>* */
+    if ((tmp = asc2nibble(cs[idx + 1])) > 0x0F) return 0;
+
+    cf->flags = tmp;
+    idx += 2;
+  }
+
+  for (i = 0, dlen = 0; i < maxdlen; i++) {
+    if (cs[idx] == DATA_SEPERATOR) /* skip (optional) separator */
+      idx++;
+
+    if (idx >= len) /* end of string => end of data */
+      break;
+
+    if ((tmp = asc2nibble(cs[idx++])) > 0x0F) return 0;
+    cf->data[i] = (tmp << 4);
+    if ((tmp = asc2nibble(cs[idx++])) > 0x0F) return 0;
+    cf->data[i] |= tmp;
+    dlen++;
+  }
+  cf->len = dlen;
+
+  /* check for extra DLC when having a Classic CAN with 8 bytes payload */
+  if ((maxdlen == CAN_MAX_DLEN) && (dlen == CAN_MAX_DLEN) && (cs[idx++] == CC_DLC_DELIM)) {
+    unsigned char dlc = asc2nibble(cs[idx]);
+
+    if ((dlc > CAN_MAX_DLEN) && (dlc <= CAN_MAX_RAW_DLC)) {
+      struct can_frame *ccf = (struct can_frame *)cf;
+
+      ccf->__res1 = dlc;
+    }
+  }
+
+  return ret;
+}
+
+int SocketCan::sendFrame(string framestr) {
+  struct canfd_frame frame;
+  int                required_mtu;
+  required_mtu = parse_canframe((char *)framestr.c_str(), &frame);
+  if (!required_mtu) {
+    logger->error("Wrong CAN-frame format!");
+    return -1;
+  }
+
+  if (required_mtu > (int)CAN_MTU) {
+    logger->error("CAN-FD frames not supported!");
+    return -1;
+  }
+  logger->info("send frame:{} {} {} {} {} {}", required_mtu, frame.can_id, frame.len, frame.flags, frame.__res0, frame.__res1);
+  if (write(m_canfd, &frame, required_mtu) != required_mtu) {
+    logger->error("CAN-FD frames not supported!");
+    return -1;
+  }
+  return 0;
+}
 
 void SocketCan::dosystem(string cmd) {
   logger->info("do cmd:{}", cmd);

src/test/socketcan.hpp

@@ -47,8 +47,8 @@ using namespace core;
  *                                  CAN_FRAME                                  *
  *******************************************************************************/
 
-typedef struct can_filter can_filter_t;
-typedef struct can_frame  can_frame_t;
+typedef struct can_filter  can_filter_t;
+typedef struct canfd_frame canfd_frame_t;
 typedef enum {
   kstdFrame,
   kextFrame,
@@ -70,8 +70,8 @@ class CanFrame {
  public:
   static shared_ptr<CanFrame> createExtDataFrame(uint32_t id, uint8_t *data, size_t len);
   static shared_ptr<CanFrame> createStdDataFrame(uint32_t id, uint8_t *data, size_t len);
-  static shared_ptr<CanFrame> createExtRemoteFrame(uint32_t id, uint8_t dlc);
-  static shared_ptr<CanFrame> createStdRemoteFrame(uint32_t id, uint8_t dlc);
+  static shared_ptr<CanFrame> createExtRemoteFrame(uint32_t id);
+  static shared_ptr<CanFrame> createStdRemoteFrame(uint32_t id);
 
   void             setId(uint32_t id);
   uint32_t         getId();
@@ -80,7 +80,7 @@ class CanFrame {
   can_identifier_t getCanIdentifier();
   void             setCanIdentifier(can_identifier_t canIdentifier);
 
-  can_frame_t getCanFrame();
+  canfd_frame_t getCanFrame();
 };
 
 /*******************************************************************************
@@ -102,6 +102,17 @@ class SocketCan {
   SocketCan(){};
   bool initialize(string canName, int bitrate, vector<can_filter_t> filters);
   int  sendFrame(shared_ptr<CanFrame> frame);
+  /**
+   * @brief 发送can帧
+   *
+   * @param frame
+   *    frame="11111111#1122334455667788"  ID=0x11111111,帧类型=数据帧,帧格式=扩展帧,数据长度=8,数据=1122334455667788
+   *    frame="111#1122334455667788"       ID=0x111,     帧类型=数据帧,帧格式=标准帧,数据长度=8,数据=1122334455667788
+   *    frame="111#R"                      ID=0x111,     帧类型=远程帧,帧格式=标准帧,数据长度=0
+   *    frame="11111111#R"                 ID=0x11111111,帧类型=远程帧,帧格式=扩展帧,数据长度=0
+   * @return int
+   */
+  int  sendFrame(string frame);
   void startListen();
   ~SocketCan() {
     if (m_canfd > 0) {
@@ -113,5 +124,6 @@ class SocketCan {
   void dosystem(string cmd);
   void setCanBitrate(string canName, int bitrate);
   bool findCan(string canName);
+  int  parse_canframe(char *cs, struct canfd_frame *cf);
 };
 }  // namespace iflytop

src/test/test_can.cpp

@@ -57,6 +57,50 @@ class Main {
   }
   void onSIGINT() { exit(0); }
   int  main(int argCount, char* argValues[]) {
+#if 0
+    int                 s, nbytes;
+    struct sockaddr_can addr;
+    struct ifreq        ifr;
+    struct can_frame    frame[2] = {{0}};
+    s                            = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+    printf("send_s = %d\n", s);
+    strcpy(ifr.ifr_name, "can0");
+    ioctl(s, SIOCGIFINDEX, &ifr);
+    addr.can_family  = AF_CAN;
+    addr.can_ifindex = ifr.ifr_ifindex;
+    bind(s, (struct sockaddr*)&addr, sizeof(addr));
+
+    setsockopt(s, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
+
+    frame[0].can_id  = 0x11;
+    frame[0].can_dlc = 1;
+    frame[0].data[0] = 'Y';
+    frame[1].can_id  = 0x410;
+    frame[1].can_dlc = 1;
+    frame[1].data[0] = 'N';
+
+    while (1) {
+      nbytes = write(s, &frame[0], sizeof(frame[0]));
+      if (nbytes != sizeof(frame[0])) {
+        printf("Send Error frame[0]!\n");
+        break;
+      }
+
+      sleep(1);
+
+      nbytes = write(s, &frame[1], sizeof(frame[1]));
+      if (nbytes != sizeof(frame[1])) {
+        printf("Send Error frame[1]!\n");
+        break;
+      }
+
+      sleep(1);
+    }
+
+    close(s);
+    while(1) sleep(1000);
+#endif
+
     socketCan.reset(new SocketCan());
     socketCan->initialize("can0", 500000, {});
 
@@ -70,15 +114,23 @@ class Main {
     ThisThread thisThread;
     int        i = 0;
     while (!thisThread.getExitFlag()) {
-      thisThread.sleepForMs(1000);
-      uint8_t data[8] = {0};
-      data[0]++;
-      shared_ptr<CanFrame> canframe = CanFrame::createExtDataFrame(i++, data, 8);
-      int                  ret      = socketCan->sendFrame(canframe);
-      if (ret < 0) {
-        logger->error("sendFrame failed");
+      thisThread.sleepForMs(10);
+      uint8_t              data[8]  = {1, 2, 3, 4, 5, 6, 7, 8};
+      shared_ptr<CanFrame> canframe = CanFrame::createExtDataFrame(0x11111111, data, 8);
+      logger->info("TX Frame {}", i);
+      socketCan->sendFrame(canframe);
+      i++;
+      if (i == 10) {
+        break;
       }
+
+#if 0
+      socketCan->sendFrame("11111111#1122334455667788");
       logger->info("TX Frame");
+#endif
+    }
+    while (true) {
+      sleep(1000);
     }
 
     return 0;