Fix mistakes regarding timestamps configuration

packet-exchange/src/send_packet.c

@@ -41,9 +41,9 @@
 #define MESSAGE ((uint32_t)0x00FACADE)
 #define NSEC_PER_SEC 1000000000
 
-static int process_socket_error_queue(int fd);
+static int process_socket_error_queue();
 static void print_timestamps(struct msghdr *msg);
-static void process_timestamps(int sock);
+static void process_timestamps(int recvmsg_flags);
 
 static int so_priority = 3;
 static struct sock_txtime sk_txtime;
@@ -130,6 +130,8 @@ void send_udp_packet(int use_etf, int use_timestamps, uint64_t txtime,
       .fd = fd,
   };
   int sendmsgerr;
+  int res;
+  fd_set readfs;
 
   memset(&sin, 0, sizeof(sin));
   sin.sin_family = AF_INET;
@@ -163,34 +165,42 @@ void send_udp_packet(int use_etf, int use_timestamps, uint64_t txtime,
     error(EXIT_FAILURE, errno, "sendmsg failed, ret value: %d\n", sendmsgerr);
 
   if(use_timestamps) {
-#ifdef DEBUG
-    printf("Polling for timestamps...\n");
-#endif
-    poll(&poll_fd, 1, 0);
-    process_timestamps(fd);
+
+    FD_ZERO(&readfs);
+    FD_SET(fd, &readfs);
+
+    res = select(fd + 1, &readfs, 0, NULL, NULL);
+    printf("select returned: %d, %s\n", res, res < 0 ? strerror(errno) : "success");
+    if (res > 0) {
+      if (FD_ISSET(fd, &readfs)) printf("ready for reading\n");
+      process_timestamps(MSG_ERRQUEUE);
+    }
   }
 }
 
-static void process_timestamps(int sock) {
+static void process_timestamps(int recvmsg_flags) {
+  char data[256];
   struct msghdr msg;
   struct iovec entry;
+  struct sockaddr_in from_addr;
   struct {
     struct cmsghdr cm;
     char control[512];
   } control;
-
-#ifdef DEBUG
-  printf("process_timestamps\n");
-#endif
+  int res;
 
   memset(&msg, 0, sizeof(msg));
   msg.msg_iov = &entry;
   msg.msg_iovlen = 1;
+  entry.iov_base = data;
+  entry.iov_len = sizeof(data);
+  msg.msg_name = (caddr_t)&from_addr;
+  msg.msg_namelen = sizeof(from_addr);
   msg.msg_control = &control;
   msg.msg_controllen = sizeof(control);
 
-  if (recvmsg(sock, &msg, MSG_DONTWAIT) == -1) {
-    fprintf(stderr, "recvmsg failed");
+  if (recvmsg(fd, &msg, recvmsg_flags | MSG_DONTWAIT) == -1) {
+    fprintf(stderr, "recvmsg failed\n");
   }
   else {
     print_timestamps(&msg);
@@ -254,7 +264,7 @@ static void print_timestamps(struct msghdr *msg) {
 /*
  * Code from scheduled_tx_tools
  */
-static int process_socket_error_queue(int fd) {
+static int process_socket_error_queue() {
   uint8_t msg_control[CMSG_SPACE(sizeof(struct sock_extended_err))];
   unsigned char err_buffer[sizeof(tx_buffer)];
   struct sock_extended_err *serr;