Process socket's error queues

packet-exchange/src/send_packet.c

@@ -93,6 +93,8 @@ int send_udp_packet(const char *server_ip) {
  * ETF qdisc section
  */
 
+static int process_socket_error_queue(int fd);
+
 static int so_priority = 3;
 static struct sock_txtime sk_txtime;
 static unsigned char tx_buffer[1024] = "Hi";
@@ -164,7 +166,10 @@ void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip) {
   struct cmsghdr *cmsg;
   struct msghdr msg;
   struct iovec iov;
-  int sendmsgerr;
+  struct pollfd poll_fd = {
+      .fd = fd,
+  };
+  int sendmsgerr, pollerr;
 
   memset(&sin, 0, sizeof(sin));
   sin.sin_family = AF_INET;
@@ -196,4 +201,59 @@ void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip) {
   sendmsgerr = sendmsg(fd, &msg, 0);
   if (sendmsgerr < 0)
     error(EXIT_FAILURE, errno, "sendmsg failed, ret value: %d\n", sendmsgerr);
+
+  pollerr = poll(&poll_fd, 1, 0);
+  // If the poll revents containts a POLLERR flag
+  if (pollerr == 1 && (poll_fd.revents & POLLERR)) {
+    process_socket_error_queue(fd);
+  }
+}
+
+/*
+ * Code from scheduled_tx_tools
+ */
+static int process_socket_error_queue(int fd) {
+  uint8_t msg_control[CMSG_SPACE(sizeof(struct sock_extended_err))];
+  unsigned char err_buffer[sizeof(tx_buffer)];
+  struct sock_extended_err *serr;
+  struct cmsghdr *cmsg;
+  __u64 tstamp = 0;
+
+  struct iovec iov = {.iov_base = err_buffer, .iov_len = sizeof(err_buffer)};
+  struct msghdr msg = {.msg_iov = &iov,
+                       .msg_iovlen = 1,
+                       .msg_control = msg_control,
+                       .msg_controllen = sizeof(msg_control)};
+
+  if (recvmsg(fd, &msg, MSG_ERRQUEUE) == -1) {
+    fprintf(stderr, "recvmsg failed");
+    return -1;
+  }
+
+  cmsg = CMSG_FIRSTHDR(&msg);
+  while (cmsg != NULL) {
+    serr = (void *)CMSG_DATA(cmsg);
+    if (serr->ee_origin == SO_EE_ORIGIN_TXTIME) {
+      tstamp = ((__u64)serr->ee_data << 32) + serr->ee_info;
+
+      switch (serr->ee_code) {
+        case SO_EE_CODE_TXTIME_INVALID_PARAM:
+          fprintf(stderr,
+                  "packet with tstamp %llu dropped due to invalid params\n",
+                  tstamp);
+          return 0;
+        case SO_EE_CODE_TXTIME_MISSED:
+          fprintf(stderr,
+                  "packet with tstamp %llu dropped due to missed deadline\n",
+                  tstamp);
+          return 0;
+        default:
+          return -1;
+      }
+    }
+
+    cmsg = CMSG_NXTHDR(&msg, cmsg);
+  }
+
+  return 0;
 }