API tools faq
paste
Advertisement
theosib

UDP datagram timestamps

theosib
Nov 15th, 2017
1,297
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C 14.54 KB | None | 0 0
raw download clone embed print report
  1.  
  2. #define __USE_MISC
  3. #include <stdio.h>
  4. #include <string.h>
  5. #include <stdlib.h>
  6. #include <arpa/inet.h>
  7. #include <sys/socket.h>
  8. #include <stdint.h>
  9. #include <sys/types.h>
  10. #include <unistd.h>
  11. #include <string.h>
  12. #include <errno.h>
  13. #include <sys/time.h>
  14. #include <linux/net_tstamp.h>
  15. #include <linux/errqueue.h>
  16. #include <linux/sockios.h>
  17. #include <net/if.h>
  18. #include <sys/ioctl.h>
  19.  
  20.  
  21. const int UDP_MAX_LENGTH = 1500;
  22.  
  23. typedef struct {
  24.     int fd, port, err_no;
  25.     struct sockaddr_in local, remote;
  26.     struct timeval time_kernel, time_user;
  27.     int64_t prev_serialnum;    // Message serialnum corresponding to timestamps
  28. } socket_info;
  29.  
  30.  
  31. int setup_udp_receiver(socket_info *inf, int port)
  32. {
  33.     int r;
  34.     int timestampOn;
  35.     int on = 1;
  36.     struct ifreq ifr;
  37.     struct hwtstamp_config hwc;
  38.    
  39.     inf->port = port;
  40.     inf->fd = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  41.     if (inf->fd < 0) {
  42.         inf->err_no = errno;
  43.         fprintf(stderr, "setup_udp_server: socket failed: %s\n", strerror(inf->err_no));
  44.         return inf->fd;
  45.     }
  46.  
  47.     /*memset(&ifr, 0, sizeof(ifr));
  48.     hwc.flags = 0;
  49.     hwc.tx_type = HWTSTAMP_TX_ON;
  50.     hwc.rx_filter = HWTSTAMP_FILTER_ALL;
  51.     ifr.ifr_data = (char*)&hwc;
  52.     r = ioctl(inf->fd, SIOCSHWTSTAMP, &ifr);
  53.     if (r < 0) {
  54.         inf->err_no = errno;
  55.         fprintf(stderr, "setup_udp_server: ioctl failed: %s\n", strerror(inf->err_no));
  56.         return r;
  57.     }*/
  58.    
  59.     //timestampOn = SOF_TIMESTAMPING_RX_SOFTWARE;
  60.     timestampOn = SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_TX_HARDWARE;
  61.     r = setsockopt(inf->fd, SOL_SOCKET, SO_TIMESTAMPING, &timestampOn, sizeof(timestampOn));
  62.     if (r < 0) {
  63.         inf->err_no = errno;
  64.         fprintf(stderr, "setup_udp_server: setsockopt failed: %s\n", strerror(inf->err_no));
  65.         return r;
  66.     }
  67.    
  68.     r = setsockopt(inf->fd, SOL_SOCKET, SO_REUSEPORT, &on, sizeof(on));
  69.     if (r < 0) {
  70.         inf->err_no = errno;
  71.         fprintf(stderr, "setup_udp_server: setsockopt2 failed: %s\n", strerror(inf->err_no));
  72.         return r;
  73.     }
  74.    
  75.     memset(&(inf->local), 0, sizeof(struct sockaddr_in));
  76.     inf->local.sin_family = AF_INET;
  77.     inf->local.sin_port = htons(port);
  78.     inf->local.sin_addr.s_addr = htonl(INADDR_ANY);
  79.    
  80.     r = bind(inf->fd, (struct sockaddr *)&(inf->local), sizeof(struct sockaddr_in));
  81.     if (r < 0) {
  82.         inf->err_no = errno;
  83.         fprintf(stderr, "setup_udp_server: bind failed: %s\n", strerror(inf->err_no));
  84.         return r;
  85.     }
  86.    
  87.     inf->prev_serialnum = -1;
  88.    
  89.     return 0;
  90. }
  91.  
  92.  
  93.  
  94. int setup_udp_sender(socket_info *inf, int port, char *address)
  95. {
  96.     int r;
  97.     int timestampOn;
  98.     int on = 1;
  99.     struct ifreq ifr;
  100.     struct hwtstamp_config hwc;
  101.    
  102.     inf->port = port;
  103.     inf->fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
  104.     if (inf->fd < 0) {
  105.         inf->err_no = errno;
  106.         fprintf(stderr, "setup_udp_client: socket failed: %s\n", strerror(inf->err_no));
  107.         return inf->fd;
  108.     }
  109.  
  110.     /*memset(&ifr, 0, sizeof(ifr));
  111.     hwc.flags = 0;
  112.     hwc.tx_type = HWTSTAMP_TX_ON;
  113.     hwc.rx_filter = HWTSTAMP_FILTER_ALL;
  114.     ifr.ifr_data = (char*)&hwc;
  115.     r = ioctl(inf->fd, SIOCSHWTSTAMP, &ifr);
  116.     if (r < 0) {
  117.         inf->err_no = errno;
  118.         fprintf(stderr, "setup_udp_server: ioctl failed: %s\n", strerror(inf->err_no));
  119.         return r;
  120.     }*/
  121.  
  122.     memset(&(inf->remote), 0, sizeof(struct sockaddr_in));
  123.     inf->remote.sin_family = AF_INET;
  124.     inf->remote.sin_port = htons(port);
  125.  
  126.     timestampOn = SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_RX_HARDWARE | SOF_TIMESTAMPING_TX_HARDWARE;
  127.     //timestampOn = SOF_TIMESTAMPING_TX_SOFTWARE;
  128.     r = setsockopt(inf->fd, SOL_SOCKET, SO_TIMESTAMPING, &timestampOn, sizeof(timestampOn));
  129.     if (r < 0) {
  130.         inf->err_no = errno;
  131.         fprintf(stderr, "setup_udp_server: setsockopt failed: %s\n", strerror(inf->err_no));
  132.         return r;
  133.     }
  134.  
  135.     /*r = setsockopt(inf->fd, SOL_SOCKET, SO_REUSEPORT, &on, sizeof(on));
  136.     if (r < 0) {
  137.         inf->err_no = errno;
  138.         fprintf(stderr, "setup_udp_server: setsockopt2 failed: %s\n", strerror(inf->err_no));
  139.         return r;
  140.     }*/
  141.    
  142.     r = setsockopt(inf->fd, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on));
  143.     if (r < 0) {
  144.         inf->err_no = errno;
  145.         fprintf(stderr, "setup_udp_server: setsockopt3 failed: %s\n", strerror(inf->err_no));
  146.         return r;
  147.     }
  148.          
  149.     r = inet_aton(address, &(inf->remote.sin_addr));
  150.     if (r == 0) {
  151.         fprintf(stderr, "setup_udp_client: inet_aton failed\n");
  152.         inf->err_no = 0;
  153.         return -1;
  154.     }
  155.    
  156.     memset(&(inf->local), 0, sizeof(struct sockaddr_in));
  157.     inf->local.sin_family = AF_INET;
  158.     inf->local.sin_port = htons(0);
  159.     inf->local.sin_addr.s_addr = htonl(INADDR_ANY);
  160.    
  161.     /*r = bind(inf->fd, (struct sockaddr *)&(inf->local), sizeof(struct sockaddr_in));
  162.     if (r < 0) {
  163.         inf->err_no = errno;
  164.         fprintf(stderr, "setup_udp_server: bind failed: %s\n", strerror(inf->err_no));
  165.         return r;
  166.     }*/
  167.  
  168.     inf->prev_serialnum = -1;
  169.    
  170.     return 0;
  171. }
  172.  
  173. void handle_time(struct msghdr* msg)
  174. {
  175.     struct timespec* ts = NULL;
  176.     struct cmsghdr* cmsg;
  177.     struct sock_extended_err *ext;
  178.  
  179.     for( cmsg = CMSG_FIRSTHDR(msg); cmsg; cmsg = CMSG_NXTHDR(msg,cmsg) ) {
  180.         printf("level=%d, type=%d, len=%zu\n", cmsg->cmsg_level, cmsg->cmsg_type, cmsg->cmsg_len);
  181.  
  182.         if ( cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_RECVERR) {
  183.             ext = (struct sock_extended_err *)CMSG_DATA(cmsg);
  184.             printf("errno=%d, origin=%d\n", ext->ee_errno, ext->ee_origin);
  185.             continue;
  186.         }
  187.  
  188.         if( cmsg->cmsg_level != SOL_SOCKET )
  189.             continue;
  190.  
  191.         switch( cmsg->cmsg_type ) {
  192.         case SO_TIMESTAMPNS:
  193.             ts = (struct timespec*) CMSG_DATA(cmsg);
  194.             break;
  195.         case SO_TIMESTAMPING:
  196.             ts = (struct timespec*) CMSG_DATA(cmsg);
  197.             break;
  198.         default:
  199.             /* Ignore other cmsg options */
  200.             break;
  201.         }
  202.     }
  203.     printf("End messages\n");
  204.  
  205.     //print_time(ts);
  206. }
  207.  
  208.  
  209. int udp_receive(socket_info *inf, char *buf, int len)
  210. {
  211.     int recv_len;
  212.     struct msghdr   msg;
  213.     struct iovec    iov;
  214.  
  215.     memset(&msg, 0, sizeof(msg));
  216.     memset(&iov, 0, sizeof(iov));
  217.    
  218.     // Space for control message info plus timestamp
  219.     char ctrl[2048];
  220.     memset(ctrl, 0, sizeof(ctrl));
  221.     //struct cmsghdr *cmsg = (struct cmsghdr *) &ctrl;
  222.    
  223.     // Ancillary data buffer and length
  224.     msg.msg_control      = (char *) ctrl;
  225.     msg.msg_controllen   = sizeof(ctrl);    
  226.  
  227.     // Dest address info
  228.     msg.msg_name         = (struct sockaddr *) &(inf->remote);
  229.     msg.msg_namelen      = sizeof(struct sockaddr_in);
  230.    
  231.     // Array of data buffers (scatter/gather)
  232.     msg.msg_iov          = &iov;
  233.     msg.msg_iovlen       = 1;
  234.    
  235.     // Data buffer pointer and length
  236.     iov.iov_base         = buf;
  237.     iov.iov_len          = len;
  238.    
  239. //    recv_len = recvfrom(inf->fd, buf, len, 0, (struct sockaddr *) &(inf->remote), &(inf->remote_len));
  240.     recv_len = recvmsg(inf->fd, &msg, 0);
  241.     gettimeofday(&(inf->time_user), NULL);
  242.    
  243.     if (recv_len < 0) {
  244.         inf->err_no = errno;
  245.         fprintf(stderr, "udp_receive: recvfrom failed: %s\n", strerror(inf->err_no));
  246.     }
  247.  
  248.     handle_time(&msg);
  249.    
  250.     /*if (cmsg->cmsg_level == SOL_SOCKET &&
  251.         cmsg->cmsg_type  == SCM_TIMESTAMP &&
  252.         cmsg->cmsg_len   == CMSG_LEN(sizeof(struct timeval))) {
  253.         memcpy(&(inf->time_kernel), CMSG_DATA(cmsg), sizeof(struct timeval));
  254.     } else {
  255.         fprintf(stderr, "Didn't get kernel time:  level=%d, type=%d, len=%zu\n", cmsg->cmsg_level, cmsg->cmsg_type, cmsg->cmsg_len);
  256.     }*/
  257.     //inf->remote_len = msg.msg_namelen;
  258.    
  259.     return recv_len;
  260. }
  261.  
  262. char junk_buf[65536];
  263.  
  264. int udp_send(socket_info *inf, char *buf, int len)
  265. {
  266.     int send_len, r, i;
  267.     struct msghdr   msg;
  268.     struct iovec    iov;
  269.    
  270.     memset(&msg, 0, sizeof(msg));
  271.     memset(&iov, 0, sizeof(iov));
  272.  
  273.     // Space for control message info plus timestamp
  274.     char ctrl[2048];
  275.     memset(ctrl, 0, sizeof(ctrl));
  276.     //struct cmsghdr *cmsg = (struct cmsghdr *) &ctrl;
  277.    
  278.     // Ancillary data buffer and length
  279.     //msg.msg_control      = (char *) ctrl;
  280.     //msg.msg_controllen   = sizeof(ctrl);    
  281.  
  282.     // Dest address info
  283.     msg.msg_name         = (struct sockaddr *) &(inf->remote);
  284.     msg.msg_namelen      = sizeof(struct sockaddr_in);
  285.    
  286.     // Array of data buffers (scatter/gather)
  287.     msg.msg_iov          = &iov;
  288.     msg.msg_iovlen       = 1;
  289.    
  290.     // Data buffer pointer and length
  291.     iov.iov_base         = buf;
  292.     iov.iov_len          = len;
  293.    
  294.     // send_len = sendto(inf->fd, buf, len, 0, (struct sockaddr *) &(inf->local), sizeof(struct sockaddr_in));
  295.     gettimeofday(&(inf->time_user), NULL);
  296.     send_len = sendmsg(inf->fd, &msg, 0);
  297.     if (send_len < 0) {
  298.         inf->err_no = errno;
  299.         fprintf(stderr, "udp_send: sendmsg failed: %s\n", strerror(inf->err_no));
  300.     }
  301.  
  302.     memset(&msg, 0, sizeof(msg));
  303.     memset(&iov, 0, sizeof(iov));
  304.     memset(ctrl, 0, sizeof(ctrl));
  305.     msg.msg_control      = (char *) ctrl;
  306.     msg.msg_controllen   = sizeof(ctrl);    
  307.     msg.msg_name         = (struct sockaddr *) &(inf->remote);
  308.     msg.msg_namelen      = sizeof(struct sockaddr_in);
  309.     msg.msg_iov          = &iov;
  310.     msg.msg_iovlen       = 1;
  311.     iov.iov_base         = junk_buf;
  312.     iov.iov_len          = sizeof(junk_buf);
  313.  
  314.     for (;;) {
  315.         r = recvmsg(inf->fd, &msg, MSG_ERRQUEUE);
  316.         /*for (i=0; i<2048; i++) {
  317.             printf("%02X ", ctrl[i]);
  318.             if ((i & 31) == 31) printf("\n");
  319.         }*/
  320.         if (r<0) {
  321.             fprintf(stderr, "Didn't get kernel time\n");
  322.             return send_len;
  323.         }
  324.    
  325.         printf("recvmsg returned %d\n", r);
  326.         handle_time(&msg);
  327.     }
  328.     /*if (cmsg->cmsg_level == SOL_SOCKET &&
  329.         cmsg->cmsg_type  == SCM_TIMESTAMP &&
  330.         cmsg->cmsg_len   == CMSG_LEN(sizeof(struct timeval))) {
  331.         memcpy(&(inf->time_kernel), CMSG_DATA(cmsg), sizeof(struct timeval));
  332.     } else {
  333.         fprintf(stderr, "Didn't get kernel time:  level=%d, type=%d, len=%zu\n", cmsg->cmsg_level, cmsg->cmsg_type, cmsg->cmsg_len);
  334.     }*/
  335.    
  336.     return send_len;        
  337. }
  338.  
  339.  
  340. typedef struct {
  341.     int64_t serialnum;              // Message index, incremented by sender for each message
  342.    
  343.     int64_t user_time_serialnum;    // Serial number of earlier message
  344.     int64_t user_time;              // Microsecond system time before sender called sendto()
  345.    
  346.     int64_t kernel_time_serialnum;  // Serial number of earlier message
  347.     int64_t kernel_time;            // Kernel timestamp of when earlier packet was sent
  348.    
  349.     uint16_t message_bytes;         // Length of random payload following header
  350. } message_header;
  351. const int payload_max = UDP_MAX_LENGTH - sizeof(message_header);
  352.  
  353. int64_t serial_num = 0;
  354. int generate_random_message(char *buf, socket_info *inf)
  355. {
  356.     int payload_len, i, total;
  357.     message_header *header = (message_header *)buf;
  358.     char *payload = buf + sizeof(message_header);
  359.    
  360.     header->user_time_serialnum = inf->prev_serialnum;
  361.     header->user_time = inf->time_user.tv_sec * 1000000000L + inf->time_user.tv_usec;
  362.     header->kernel_time_serialnum = inf->prev_serialnum;
  363.     header->kernel_time = inf->time_kernel.tv_sec * 1000000000L + inf->time_kernel.tv_usec;
  364.     header->serialnum = serial_num;    
  365.  
  366.     payload_len = random() % (payload_max+1);
  367.     for (i=0; i<payload_len; i++) {
  368.         payload[i] = random();
  369.     }
  370.     header->message_bytes = payload_len;
  371.     total = payload_len + sizeof(message_header);
  372.  
  373.     printf("%5ld: kt=%ld, ut=%ld    %5ld: s=%d\n", header->user_time_serialnum, header->kernel_time, header->user_time, header->serialnum, total);
  374.    
  375.     inf->prev_serialnum = serial_num;
  376.     serial_num++;
  377.    
  378.     return total;
  379. }
  380.  
  381.  
  382. typedef struct {
  383.     int64_t sender_user_time, sender_kernel_time;
  384.     int64_t receiver_user_time, receiver_kernel_time;
  385.     uint16_t message_bytes;
  386. } time_record;
  387.  
  388.  
  389. time_record records[10000];
  390. int num_records = 0;
  391.  
  392. void clear_records()
  393. {
  394.     memset(records, 0, sizeof(records));
  395.     num_records = 0;
  396. }
  397.  
  398. char packet_buffer[65536];
  399.  
  400. int fetch_message(socket_info *inf)
  401. {
  402.     int msg_size, i;
  403.     message_header *header = (message_header *)packet_buffer;
  404.    
  405.     msg_size = udp_receive(inf, packet_buffer, 65536);
  406.     if (msg_size < 0) exit(0);
  407.    
  408.     if (msg_size != header->message_bytes + sizeof(message_header)) {
  409.         fprintf(stderr, "Message size mismatch\n");
  410.         exit(0);
  411.     }
  412.    
  413.     records[header->serialnum].message_bytes = msg_size;
  414.     records[header->serialnum].receiver_user_time = inf->time_user.tv_sec * 1000000000L + inf->time_user.tv_usec;
  415.     records[header->serialnum].receiver_kernel_time = inf->time_kernel.tv_sec * 1000000000L + inf->time_kernel.tv_usec;
  416.     records[header->user_time_serialnum].sender_user_time = header->user_time;
  417.     records[header->kernel_time_serialnum].sender_kernel_time = header->kernel_time;
  418.    
  419.     i = header->user_time_serialnum;
  420.     printf("%5d: sut=%ld, skt=%ld, rkt=%ld, rut=%ld, s=%d\n", i, records[i].sender_user_time, records[i].sender_kernel_time,
  421.         records[i].receiver_kernel_time, records[i].receiver_user_time, (int)records[i].message_bytes);
  422.    
  423.     int num = header->user_time_serialnum + 1;
  424.     if (num > num_records) num_records = num;
  425.    
  426.     return 0;
  427. }
  428.  
  429.  
  430.  
  431. void sender_loop()
  432. {
  433.     int i, len, t;
  434.    
  435.     socket_info inf;
  436.     t = setup_udp_sender(&inf, 8000, "192.168.1.255");
  437.     if (t<0) exit(0);
  438.    
  439.     for (i=0; i<2000; i++) {
  440.         t = random() % 2000000;
  441.         usleep(t);
  442.         len = generate_random_message(packet_buffer, &inf);
  443.         udp_send(&inf, packet_buffer, len);
  444.     }
  445. }
  446.  
  447.  
  448. void receiver_loop()
  449. {
  450.     int i;
  451.    
  452.     socket_info inf;
  453.     setup_udp_receiver(&inf, 8000);
  454.    
  455.     for (int i=0; 1000; i++) {
  456.         fetch_message(&inf);
  457.     }
  458. }
  459.  
  460.  
  461. int main(int argc, char *argv[])
  462. {
  463.     clear_records();
  464.    
  465.     if (argc != 2) {
  466.         fprintf(stderr, "Missing option\n");
  467.         return 0;
  468.     }
  469.    
  470.     if (0==strcmp(argv[1], "-s")) {
  471.         sender_loop();
  472.     } else
  473.     if (0==strcmp(argv[1], "-r")) {
  474.         receiver_loop();
  475.     } else {
  476.         fprintf(stderr, "Unknown option\n");
  477.     }
  478.    
  479.     return 0;
  480. }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
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
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!