add can

6/CANsmit3/isotpterm.c

@@ -0,0 +1,183 @@
+/*
+ * isotpterm.c - interactive terminal over isotp
+ */
+
+#include <errno.h>
+#include <libgen.h>
+#include <linux/can.h>
+#include <linux/can/isotp.h>
+#include <net/if.h>
+#include <netinet/in.h>
+#include <signal.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <sys/types.h>
+#include <sys/uio.h>
+#include <sys/wait.h>
+#include <time.h>
+#include <unistd.h>
+
+#define NO_CAN_ID 0xFFFFFFFFU
+#define MAX_PDU_LENGTH 8000
+
+void print_usage(char *prg) {
+  fprintf(stderr,
+          "\nUsage: %s -s <can_id> -d <can_id> [options] <CAN interface>\n",
+          prg);
+  fprintf(stderr, "Options:\n");
+  fprintf(stderr,
+          "         -s <can_id>  * (source can_id. Use 8 digits for extended "
+          "IDs)\n");
+  fprintf(stderr,
+          "         -d <can_id>  * (destination can_id. Use 8 digits for "
+          "extended IDs)\n");
+  fprintf(stderr, "\n");
+}
+
+int main(int argc, char **argv) {
+  extern int optind, opterr, optopt;
+  int opt;
+
+  int sc = 0; /* (C)AN socket */
+  struct sockaddr_can caddr;
+  static struct can_isotp_options opts;
+
+  socklen_t caddrlen = sizeof(caddr);
+  fd_set readfds;
+
+  int nbytes;
+  int ret = 0;
+  char *fgetsret = NULL;
+
+  char txmsg[MAX_PDU_LENGTH];
+  char rxmsg[MAX_PDU_LENGTH];
+
+  /* mark missing mandatory commandline options as missing */
+  caddr.can_addr.tp.tx_id = caddr.can_addr.tp.rx_id = NO_CAN_ID;
+
+  while ((opt = getopt(argc, argv, "s:d:?")) != -1) {
+    switch (opt) {
+      case 's':
+        caddr.can_addr.tp.tx_id = strtoul(optarg, (char **)NULL, 16);
+        if (strlen(optarg) > 7) caddr.can_addr.tp.tx_id |= CAN_EFF_FLAG;
+        break;
+
+      case 'd':
+        caddr.can_addr.tp.rx_id = strtoul(optarg, (char **)NULL, 16);
+        if (strlen(optarg) > 7) caddr.can_addr.tp.rx_id |= CAN_EFF_FLAG;
+        break;
+
+      case '?':
+        print_usage(basename(argv[0]));
+        ret = 1; /* no proper operation (for non-interactive users) */
+        goto exit;
+
+      default:
+        fprintf(stderr, "Unknown option %c\n", opt);
+        print_usage(basename(argv[0]));
+        ret = 1;
+        goto exit;
+    }
+  }
+
+  if ((argc - optind != 1) || (caddr.can_addr.tp.tx_id == NO_CAN_ID) ||
+      (caddr.can_addr.tp.rx_id == NO_CAN_ID)) {
+    print_usage(basename(argv[0]));
+    ret = -EINVAL;
+    goto exit;
+  }
+
+  if ((sc = socket(PF_CAN, SOCK_DGRAM, CAN_ISOTP)) < 0) {
+    perror("socket");
+    ret = sc;
+    goto exit;
+  }
+
+  opts.flags = CAN_ISOTP_WAIT_TX_DONE;
+  setsockopt(sc, SOL_CAN_ISOTP, CAN_ISOTP_OPTS, &opts, sizeof(opts));
+
+  caddr.can_family = AF_CAN;
+  caddr.can_ifindex = if_nametoindex(argv[optind]);
+
+  ret = bind(sc, (struct sockaddr *)&caddr, caddrlen);
+  if (ret < 0) {
+    perror("bind");
+    goto exit;
+  }
+
+  while (1) {
+    FD_ZERO(&readfds);
+    FD_SET(STDIN_FILENO, &readfds);
+    FD_SET(sc, &readfds);
+
+    ret = select(sc + 1, &readfds, NULL, NULL, NULL);
+    if (ret < 0) {
+      perror("select");
+      goto exit;
+    }
+
+    if (FD_ISSET(sc, &readfds)) {
+      nbytes = read(sc, rxmsg, MAX_PDU_LENGTH - 1);
+
+      if (nbytes < 1) {
+        perror("read from isotp socket");
+        ret = nbytes;
+        goto exit;
+      }
+
+      rxmsg[nbytes] = 0; /* terminate string */
+      printf("%s", rxmsg);
+
+      if(strncmp(rxmsg, "\nwopr", 4)==0){
+	      send(sc, "falken\n", 7 , 0);
+      }
+      if(strncmp(rxmsg, "pass", 4)==0){
+	      send(sc, "Joshua\n", 7 , 0);
+      }
+      char *s = strstr(rxmsg, "Test#");
+      char c;
+      int j = 0;
+      if(s){
+	      printf("detected: %c\n", s[18]);
+	      c = s[18];
+	      while(s[0] != '\n')
+		      s++;
+
+	      s = strstr(rxmsg, "\n'");
+	      for(int i=0; i<strlen(s); i++)
+		      if(s[i] == c)
+			      j++;
+
+	      char msg[10];
+	      sprintf(msg, "%d\n", j);
+	      printf("aswering: %d\n", j);
+
+	      send(sc, msg, strlen(msg)+1, 0);
+      }
+      
+      fflush(stdout);
+
+    } else if (FD_ISSET(STDIN_FILENO, &readfds)) {
+      fgetsret = fgets(txmsg, MAX_PDU_LENGTH, stdin);
+      if (fgetsret == NULL) {
+        ret = 0;
+        goto exit;
+      }
+
+      nbytes = send(sc, txmsg, strlen(txmsg) + 1, 0);
+      if (nbytes != strlen(txmsg) + 1) {
+        perror("write to isotp socket");
+        ret = nbytes;
+        goto exit;
+      }
+    }
+  }
+
+exit:
+  close(sc);
+
+  return ret;
+}