The first partially working version - objects assignment is incorrect still.

[socketcan-simulink.git] / blocks / tlc_c / sfunction_cansetup.tlc

diff --git a/blocks/tlc_c/sfunction_cansetup.tlc b/blocks/tlc_c/sfunction_cansetup.tlc
index f2eec3498b0e35c97327e485af301b982452bdff..f228c6f182bfd319f0e82756ec4d4b38a3d4ca09 100644 (file)
--- a/blocks/tlc_c/sfunction_cansetup.tlc
+++ b/blocks/tlc_c/sfunction_cansetup.tlc
@@ -103,6 +103,7 @@
                struct ert_can_tx_config *tx_config;
                struct ert_can_rx_config *rx_config;
                struct ert_can_channel_config *channel_config;
+               const char **channel_ifname;
        };
 
        #define CAN_TX_COUNT %<Rpp.Can.Tx.NumBlocks>
@@ -161,25 +162,118 @@
          %endforeach
        };
 
+       const char *ert_can_channel_ifname[] = {
+               "can0", /*FIXME - for now skip this */
+               "can0",
+               "can1",
+               "can2",
+               "can3"
+       };
+
        const struct ert_can_config can_config = {
                .num_tx_obj = CAN_TX_COUNT,
                .num_rx_obj = CAN_RX_COUNT,
                .tx_config = tx_config,
                .rx_config = rx_config,
-               .channel_config = can_channel_config
+               .channel_config = can_channel_config,
+               .channel_ifname = ert_can_channel_ifname
        };
 
-        int can_rx_handles[CAN_RX_COUNT];
+        int can_tx_handles[CAN_TX_COUNT];
 
-        int can_tx_handles[CAN_RX_COUNT];
+        int can_rx_handles[CAN_RX_COUNT];
 
        %closefile buffer
        %<LibSetSourceFileSection(LibGetModelDotCFile(), "Declarations", buffer)>
-       int returnstatus;
+       {
+               int idx;
+
+               for (idx = 0; idx < CAN_TX_COUNT; idx++) {
+                       struct ert_can_tx_config *cfg = &can_config.tx_config[idx];
+                       const char *ifname = can_config.channel_ifname[cfg->channel];
+                       int fd;
+                       struct sockaddr_can addr;
+                       struct ifreq ifr;
+
+                       fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+                       if (fd == -1) {
+                               printf("CAN socket create error.\n");
+                               exit(1);
+                       }
+                       if(fcntl(fd, F_SETFL, O_NONBLOCK) < 0) {
+                               printf("CAN socket set O_NONBLOCK error.\n");
+                               exit(1);
+                       }
+                       memset(&addr, 0, sizeof(addr));
+                       addr.can_family = AF_CAN;
+                       strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name));
+                       if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
+                               printf("CAN socket ioctl SIOCGIFINDEX error.\n");
+                               exit(1);
+                       }
+                       addr.can_ifindex = ifr.ifr_ifindex;
+
+                       /* disable default receive filter on this RAW socket */
+                       setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
+
+                       if (bind(fd, (struct sockaddr *)(void*)&addr, sizeof(addr)) < 0) {
+                               printf("CAN socket bind error.\n");
+                               exit(1);
+                       }
+                       printf("rx opening %s\n", ifname);
+                       can_tx_handles[idx] = fd;
+               }
+
+               for (idx = 0; idx < CAN_RX_COUNT; idx++) {
+                       struct ert_can_rx_config *cfg = &can_config.rx_config[idx];
+                       const char *ifname = can_config.channel_ifname[cfg->channel];
+                       int fd;
+                       struct sockaddr_can addr;
+                       struct ifreq ifr;
+                       struct can_filter sc_filter;
+
+                       fd = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+                       if (fd == -1) {
+                               printf("CAN socket create error.\n");
+                               exit(1);
+                       }
+                       if(fcntl(fd, F_SETFL, O_NONBLOCK) < 0) {
+                               printf("CAN socket set O_NONBLOCK error.\n");
+                               exit(1);
+                       }
+                       memset(&addr, 0, sizeof(addr));
+                       addr.can_family = AF_CAN;
+                       strncpy(ifr.ifr_name, ifname, sizeof(ifr.ifr_name));
+                       if (ioctl(fd, SIOCGIFINDEX, &ifr) < 0) {
+                               printf("CAN socket ioctl SIOCGIFINDEX error.\n");
+                               exit(1);
+                       }
+                       addr.can_ifindex = ifr.ifr_ifindex;
+
+                       /* disable default receive filter on this RAW socket */
+                       setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, NULL, 0);
 
-       returnstatus = rpp_can_init(&can_config);
-       if (returnstatus < 0) {
-           printf("CAN driver initialization error.\n");
+                       if (bind(fd, (struct sockaddr *)(void*)&addr, sizeof(addr)) < 0) {
+                               printf("CAN socket bind error.\n");
+                               exit(1);
+                       }
+
+                       memset(&sc_filter, 0, sizeof(sc_filter));
+                       sc_filter.can_id = cfg->id;
+                       sc_filter.can_mask = CAN_EFF_MASK | CAN_EFF_FLAG | CAN_RTR_FLAG;
+                       if (cfg->id_type == ERT_CAN_EXTENDED)
+                               sc_filter.can_id |= CAN_EFF_FLAG;
+                       else if(cfg->id_type == ERT_CAN_MIXED)
+                               sc_filter.can_mask &= ~CAN_EFF_FLAG;
+
+                       if (setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FILTER, &sc_filter, sizeof(sc_filter)) < 0) {
+                               printf("CAN socket setsockopt CAN_RAW_FILTER error.\n");
+                               exit(1);
+                       }
+
+                       printf("tx opening %s\n", ifname);
+                       can_rx_handles[idx] = fd;
+               }
        }
        %%else {
        %%      printf("CAN communication initialized.\n");