SHARE
TWEET

Untitled

a guest Apr 19th, 2019 160 Never
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
  1. #include <stdio.h>
  2. #include <string.h>
  3. #include <stdlib.h>
  4. #include <pthread.h>
  5. #include <sys/socket.h>
  6. #include <netinet/in.h>
  7. #include <net/if.h>
  8. #include <sys/ioctl.h>
  9. #include <fcntl.h>
  10. #include <arpa/inet.h>
  11. #include <linux/can.h>
  12. #include <linux/can/raw.h>
  13. #include <unistd.h>
  14. #include <stdbool.h>
  15.  
  16. #define MAX_CONNECTIONS 16
  17.  
  18. pthread_t can_threads[MAX_CONNECTIONS];
  19. pthread_t udp_threads[MAX_CONNECTIONS];
  20.  
  21. void *udp_listener(void *arg);
  22. void *can_listener(void *arg);
  23. void print_can_frame(struct can_frame frame);
  24.  
  25. typedef struct {
  26.     int udp_socket_from;
  27.     int udp_socket_to;
  28.     int can_socket_from;
  29.     int can_socket_to;
  30.     unsigned short port_from;
  31.     unsigned short port_to;
  32.     char * can_device_from;
  33.     char * can_device_to;
  34.     struct in_addr inet_addr_to;
  35.     struct in_addr inet_addr_from;
  36.     struct sockaddr_in udp_socket_address_from;
  37.     struct sockaddr_in udp_socket_address_to;
  38.     struct sockaddr_can can_socket_address_from;
  39.     struct sockaddr_can can_socket_address_to;
  40. } Connection;
  41.  
  42. Connection connections[MAX_CONNECTIONS];
  43.  
  44. int main() {
  45.  
  46.     connections[0].can_device_from = "vcan0";
  47.     connections[0].can_device_to = "vcan1";
  48.     connections[0].port_from = 8081;
  49.     connections[0].port_to = 8080;
  50.     connections[0].inet_addr_to.s_addr = inet_addr("127.0.0.1");
  51.     connections[0].inet_addr_from.s_addr = inet_addr("127.0.0.1");
  52.  
  53.     connections[1].can_device_from = "vcan1";
  54.     connections[1].can_device_to = "vcan0";
  55.     connections[1].port_from = 8080;
  56.     connections[1].port_to = 8081;
  57.     connections[1].inet_addr_to.s_addr = inet_addr("127.0.0.1");
  58.     connections[1].inet_addr_from.s_addr = inet_addr("127.0.0.1");
  59.  
  60.     //connections[2].can_device_from = "vcan2";
  61.     connections[2].can_device_to = "vcan2";
  62.     connections[2].port_from = 8082;
  63.     connections[2].port_to = 8082;
  64.     connections[2].inet_addr_to.s_addr = inet_addr("127.0.0.1");
  65.     connections[2].inet_addr_from.s_addr = inet_addr("127.0.0.1");
  66.  
  67.     int i = 0;
  68.     for(i = 0; i < MAX_CONNECTIONS; i++) {
  69.  
  70.         // Банальная проверка конфига
  71.         if( connections[i].can_device_from == NULL) {
  72.             break;
  73.         }
  74.  
  75.         if ((connections[i].udp_socket_from = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
  76.             fprintf(stderr, "udp socket open error");
  77.             return 0;
  78.         }
  79.         if ((connections[i].udp_socket_to = socket(AF_INET, SOCK_DGRAM, 0)) < 0) {
  80.             fprintf(stderr, "udp socket open error");
  81.             return 0;
  82.         }
  83.  
  84.         int opt_from = true;
  85.         if (setsockopt(connections[i].udp_socket_from, SOL_SOCKET, SO_REUSEADDR, (char *)&opt_from, sizeof(opt_from)) < 0) {
  86.             fprintf(stderr, "setsockopt udp error");
  87.             return 0;
  88.         }
  89.         int opt_to = true;
  90.         if (setsockopt(connections[i].udp_socket_to, SOL_SOCKET, SO_REUSEADDR, (char *)&opt_to, sizeof(opt_to)) < 0) {
  91.             fprintf(stderr, "setsockopt udp error");
  92.             return 0;
  93.         }
  94.  
  95.         connections[i].udp_socket_address_from.sin_family = AF_INET;
  96.         connections[i].udp_socket_address_from.sin_port = connections[i].port_from;
  97.         connections[i].udp_socket_address_from.sin_addr.s_addr = connections[i].inet_addr_from.s_addr;
  98.  
  99.         connections[i].udp_socket_address_to.sin_family = AF_INET;
  100.         connections[i].udp_socket_address_to.sin_port = connections[i].port_to;
  101.         connections[i].udp_socket_address_to.sin_addr.s_addr = connections[i].inet_addr_to.s_addr;
  102.  
  103.         if (bind(connections[i].udp_socket_from, (struct sockaddr *)&connections[i].udp_socket_address_from, sizeof(connections[i].udp_socket_address_from)) < 0) {
  104.             fprintf(stderr, "bind error udp");
  105.             return 0;
  106.         }
  107.         if (bind(connections[i].udp_socket_to, (struct sockaddr *)&connections[i].udp_socket_address_to, sizeof(connections[i].udp_socket_address_to)) < 0) {
  108.             fprintf(stderr, "bind error udp");
  109.             return 0;
  110.         }
  111.  
  112.         if ((connections[i].can_socket_from = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  113.             fprintf(stderr, "can socket open error");
  114.             return 0;
  115.         }
  116.         if ((connections[i].can_socket_to = socket(PF_CAN, SOCK_RAW, CAN_RAW)) < 0) {
  117.             fprintf(stderr, "can socket open error");
  118.             return 0;
  119.         }
  120.  
  121.         int recv_own_msgs = 1;
  122.         if( setsockopt(connections[i].can_socket_from, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)) < 0) {
  123.             fprintf(stderr, "setsockopt can error");
  124.             return 0;
  125.         }
  126.         if (setsockopt(connections[i].can_socket_to, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &recv_own_msgs, sizeof(recv_own_msgs)) < 0) {
  127.             fprintf(stderr, "setsockopt can error");
  128.             return 0;
  129.         }
  130.  
  131.         struct ifreq ifr_from;
  132.         strcpy(ifr_from.ifr_name, connections[i].can_device_from);
  133.         if (ioctl(connections[i].can_socket_from, SIOCGIFINDEX, &ifr_from) < 0) {
  134.             fprintf(stderr, "ioctl error");
  135.             return 0;
  136.         }
  137.         struct ifreq ifr_to;
  138.         strcpy(ifr_to.ifr_name, connections[i].can_device_to);
  139.         if (ioctl(connections[i].can_socket_to, SIOCGIFINDEX, &ifr_to) < 0)
  140.         {
  141.             fprintf(stderr, "ioctl error");
  142.             return 0;
  143.         }
  144.  
  145.         connections[i].can_socket_address_from.can_family = AF_CAN;
  146.         connections[i].can_socket_address_from.can_ifindex = ifr_from.ifr_ifindex;
  147.  
  148.         connections[i].can_socket_address_to.can_family = AF_CAN;
  149.         connections[i].can_socket_address_to.can_ifindex = ifr_to.ifr_ifindex;
  150.  
  151.         if (bind(connections[i].can_socket_from, (struct sockaddr *)&connections[i].can_socket_address_from,
  152.                  sizeof(connections[i].can_socket_address_from)) < 0)
  153.         {
  154.             fprintf(stderr, "bind error can");
  155.             return 0;
  156.         }
  157.         if (bind(connections[i].can_socket_to, (struct sockaddr *)&connections[i].can_socket_address_to,
  158.                  sizeof(connections[i].can_socket_address_to)) < 0)
  159.         {
  160.             fprintf(stderr, "bind error can");
  161.             return 0;
  162.         }
  163.  
  164.         if( pthread_create(&udp_threads[i], NULL, &udp_listener, (void *)&connections[i]) < 0) {
  165.             fprintf(stderr, "pthread create error");
  166.             return 0;
  167.         }
  168.  
  169.         if (pthread_create(&can_threads[i], NULL, &can_listener, (void *)&connections[i]) < 0) {
  170.             fprintf(stderr, "pthread create error");
  171.             return 0;
  172.         }
  173.     }
  174.  
  175.     for(i = 0; i < 2; i++) {
  176.         if( udp_threads[i] != 0 ) {
  177.             pthread_join(udp_threads[i], NULL);
  178.             pthread_join(can_threads[i], NULL);
  179.         }
  180.     }
  181.    
  182.     return 0;
  183. }
  184.  
  185. void *udp_listener(void *arg)
  186. {
  187.     Connection params = * (Connection *) arg;
  188.  
  189.     pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, 0x00);
  190.     pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, 0x00);
  191.  
  192.     struct can_frame frame;
  193.  
  194.     fprintf(stderr, "Start transfer from %s:%d to %s\n", inet_ntoa(params.inet_addr_from), params.port_from, params.can_device_to);
  195.  
  196.     socklen_t udp_address_len = sizeof(params.udp_socket_address_from);
  197.     while(1) {
  198.         if (recvfrom(params.udp_socket_from, (struct frame *)&frame, sizeof(frame), 0,
  199.             (struct sockaddr *)&params.udp_socket_address_from, &udp_address_len) < 0) {
  200.             fprintf(stderr, "recvfrom udp error");
  201.         }
  202.  
  203.         fprintf(stderr, "<- UDP %s:%d %s ", inet_ntoa(params.inet_addr_from), ntohs(params.port_from), params.can_device_from);
  204.         print_can_frame(frame);
  205.  
  206.         frame.data[0]++;
  207.  
  208.         if (sendto(params.can_socket_to, &frame, sizeof(struct can_frame), 0,
  209.             (struct sockaddr *)&params.can_socket_address_to, sizeof(params.can_socket_address_to)) < 0) {
  210.             fprintf(stderr, "send to can error");
  211.             break;
  212.         }
  213.     }
  214.  
  215.     shutdown(params.udp_socket_from, 2);
  216.     shutdown(params.can_socket_to, 2);
  217.     pthread_exit(0);
  218. }
  219.  
  220. void * can_listener(void * arg) {
  221.  
  222.     Connection params = *(Connection *)arg;
  223.  
  224.     pthread_setcanceltype(PTHREAD_CANCEL_ASYNCHRONOUS, 0x00);
  225.     pthread_setcancelstate(PTHREAD_CANCEL_ENABLE, 0x00);
  226.  
  227.     fprintf(stderr, "Start transfer from %s to %s:%d \n", params.can_device_from,
  228.             inet_ntoa(params.inet_addr_to), params.port_to);
  229.  
  230.     struct can_frame frame;
  231.  
  232.     socklen_t can_addr_len = sizeof(params.can_socket_address_from);
  233.     while (1)
  234.     {
  235.         if (recvfrom(params.can_socket_from, &frame, sizeof(frame), 0, (struct sockaddr *)&params.can_socket_address_from, &can_addr_len) < 0)
  236.         {
  237.             fprintf(stderr, "receive from can error");
  238.             break;
  239.         }
  240.  
  241.         fprintf(stderr, "<- CAN %s:%d %s ", inet_ntoa(params.udp_socket_address_to.sin_addr), params.port_to, params.can_device_to);
  242.         print_can_frame(frame);
  243.  
  244.         if (sendto(params.udp_socket_to, (struct can_frame *)&frame, (sizeof(struct can_frame) + 1), 0,
  245.                    (struct sockaddr *)&params.udp_socket_address_to, sizeof(params.udp_socket_address_to)) < 0)
  246.         {
  247.             fprintf(stderr, "send to udp error");
  248.             break;
  249.         }
  250.         sleep(1);
  251.     }
  252.  
  253.     shutdown(params.udp_socket_to, 2);
  254.     shutdown(params.can_socket_from, 2);
  255.     pthread_exit(0);
  256. }
  257.  
  258. void print_can_frame(struct can_frame frame) {
  259.  
  260.     fprintf(stderr, "ID %03X [%d] ", frame.can_id, frame.can_dlc);
  261.     for (int i = 0; i < frame.can_dlc; i++)
  262.     {
  263.         fprintf(stderr, "%02X", frame.data[i]);
  264.     }
  265.     fprintf(stderr, "\n");
  266. }
RAW Paste Data
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy. OK, I Understand
 
Top