Merge /home/pi/repo/lin/pcan_lin_config/submodule/linux-lin

[linux-lin.git] / lin_config / src / sllin_config.c

diff --git a/lin_config/src/sllin_config.c b/lin_config/src/sllin_config.c
index f2c24fa3eef4b98033c245cf2e94f78881a3e384..ffb02f5c78026749fe6dade4219a3d45392bfed3 100644 (file)
--- a/lin_config/src/sllin_config.c
+++ b/lin_config/src/sllin_config.c
@@ -15,6 +15,7 @@
 #include <linux/can/bcm.h>
 
 #include "lin_config.h"
+#include "linux/lin_bus.h"
 
 #define SLLIN_LDISC                                    25
 struct bcm_msg {
@@ -28,6 +29,68 @@ struct sllin_connection {
        char iface[IFNAMSIZ+1];
 };
 
+void sllin_ms_to_timeval(int ms, struct timeval *tv)
+{
+       tv->tv_sec = (int) ms/1000;
+       tv->tv_usec = (ms % 1000) * 1000;
+}
+
+int sllin_cache_config(struct linc_lin_state *linc_lin_state,
+                       struct sllin_connection *sllin_connection)
+{
+       int i;
+       struct ifreq ifr;
+       struct sockaddr_can addr;
+       struct can_frame frame;
+       int s;
+       int ret;
+
+       /* Create the socket */
+       s = socket(PF_CAN, SOCK_RAW, CAN_RAW);
+       if (s < 0) {
+               perror("socket()");
+               return -1;
+       }
+
+       /* Locate the interface you wish to use */
+       strcpy(ifr.ifr_name, sllin_connection->iface);
+       ioctl(s, SIOCGIFINDEX, &ifr); /* ifr.ifr_ifindex gets filled
+                                      * with that device's index */
+
+       /* Select that CAN interface, and bind the socket to it. */
+       addr.can_family = AF_CAN;
+       addr.can_ifindex = ifr.ifr_ifindex;
+       ret = bind(s, (struct sockaddr*)&addr, sizeof(addr));
+       if (ret < 0) {
+               perror("bind()");
+               return -1;
+       }
+
+       for (i = 0; i < 0x3F; i++) {
+               if (linc_lin_state->frame_entry[i].status == 1) { /* Is active */
+                       frame.can_dlc = linc_lin_state->frame_entry[i].data_len;
+                       frame.can_id = i; /* LIN ID */
+                       frame.data[0] = linc_lin_state->frame_entry[i].data[0]; /* Data */
+                       frame.data[1] = linc_lin_state->frame_entry[i].data[1]; /* Data */
+                       frame.data[2] = linc_lin_state->frame_entry[i].data[2]; /* Data */
+                       frame.data[3] = linc_lin_state->frame_entry[i].data[3]; /* Data */
+                       frame.data[4] = linc_lin_state->frame_entry[i].data[4]; /* Data */
+                       frame.data[5] = linc_lin_state->frame_entry[i].data[5]; /* Data */
+                       frame.data[6] = linc_lin_state->frame_entry[i].data[6]; /* Data */
+                       frame.data[7] = linc_lin_state->frame_entry[i].data[7]; /* Data */
+
+                       frame.can_id |= LIN_CTRL_FRAME | LIN_CACHE_RESPONSE;
+                       ret = write(s, &frame, sizeof(frame));
+                       printf("configuring frame cache; ret = %d\n", ret);
+                       //if (ret ...)
+                       //read_response(tty);
+               }
+       }
+
+       close(s);
+       return 0;
+}
+
 int sllin_bcm_config(struct linc_lin_state *linc_lin_state,
                        struct sllin_connection *sllin_connection)
 {
@@ -38,8 +101,6 @@ int sllin_bcm_config(struct linc_lin_state *linc_lin_state,
        int ret;
        int i;
 
-       //printf("tty %s to netdevice %s\n", linc_lin_state->dev, sllin_connection->iface);
-
        s = socket(PF_CAN, SOCK_DGRAM, CAN_BCM);
        if (s < 0) {
                perror("socket(): bcmsocket");
@@ -68,24 +129,29 @@ int sllin_bcm_config(struct linc_lin_state *linc_lin_state,
        }
 
        for (i = 0; i < linc_lin_state->scheduler_entries_cnt; i++) {
+               struct timeval time;
                memset(&msg, 0, sizeof(msg));
+
                msg.msg_head.nframes = 1;
                msg.msg_head.opcode = TX_SETUP;
                msg.msg_head.flags |= SETTIMER | STARTTIMER;
-               //msg.msg_head.ival2.tv_sec =  // FIXME
-               msg.msg_head.ival2.tv_usec =
-                       linc_lin_state->scheduler_entry[i].interval_ms * 1000;
-               msg.msg_head.can_id =
-                       linc_lin_state->scheduler_entry[i].lin_id | CAN_RTR_FLAG;
+               sllin_ms_to_timeval(
+                       linc_lin_state->scheduler_entry[i].interval_ms, &time);
+               msg.msg_head.ival2.tv_sec = time.tv_sec;
+               msg.msg_head.ival2.tv_usec = time.tv_usec;
+               msg.msg_head.can_id = (
+                       linc_lin_state->scheduler_entry[i].lin_id | CAN_RTR_FLAG);
                msg.frame.can_dlc = 0;
+               msg.frame.can_id = msg.msg_head.can_id;
+
+               //printf("tv_sec = %i, tv_usec = %i\n", time.tv_sec, time.tv_usec);
 
                sendto(s, &msg, sizeof(msg), 0,
                        (struct sockaddr*)&caddr, sizeof(caddr));
-               printf(".\n");
                //read_response(s); // FIXME
        }
 
-       close(s);
+       /* Do not close "s" to make BCM configuration running */
 
        printf("Configuration finished\n");
        return 0;
@@ -119,7 +185,8 @@ int sllin_config(struct linc_lin_state *linc_lin_state)
                        return -1;
                }
 
-               printf("Attached tty %s to netdevice %s\n", linc_lin_state->dev, sllin_connection.iface);
+               printf("Attached tty %s to netdevice %s\n",
+                       linc_lin_state->dev, sllin_connection.iface);
        }
 
        if (linc_lin_state->flags & SLLIN_DETACH_fl) {
@@ -130,15 +197,22 @@ int sllin_config(struct linc_lin_state *linc_lin_state)
                        return -1;
                }
 
-               printf("Detached sllin line discipline from %s\n", linc_lin_state->dev);
-               return 0;
+               printf("Detached sllin line discipline from %s\n",
+                       linc_lin_state->dev);
+
+               close(tty);
+               return LIN_EXIT_OK;
        }
 
        ret = sllin_bcm_config(linc_lin_state, &sllin_connection);
+       if (ret < 0)
+               return ret;
+
+       ret = sllin_cache_config(linc_lin_state, &sllin_connection);
+
+       /* !!! Do not close "tty" to enable newly
+          configured tty line discipline */
 
-       printf("Press any key to detach %s ...\n", linc_lin_state->dev);
-       getchar();
-       close(tty);
        return ret;
 }