ON-13142: add SOF_TIMESTAMPING_OPT_PKTINFO support

Xilinx-CNS · Jan 26, 2024 · 8a9a378 · 8a9a378
1 parent 568dbe5
commit 8a9a378
Show file tree

Hide file tree

Showing 6 changed files with 38 additions and 8 deletions.
diff --git a/src/include/ci/internal/ip.h b/src/include/ci/internal/ip.h
@@ -5297,7 +5297,8 @@ ci_sock_set_ip6_scope_id(ci_netif* ni, ci_sock_cmn* s,
 }
 #endif
 
-extern ci_ifid_t ci_rx_pkt_ifindex(ci_netif* ni, const ci_ip_pkt_fmt* pkt);
+extern ci_ifid_t ci_rx_pkt_ifindex(ci_netif* ni, const ci_ip_pkt_fmt* pkt,
+                                   bool phys_intf);
 
 
 /*********************************************************************

diff --git a/src/include/ci/internal/ip_timestamp.h b/src/include/ci/internal/ip_timestamp.h
@@ -51,6 +51,12 @@ enum {
   ONLOAD_TIMESTAMPING_FLAG_RX_COUNT = 2,
 };
 
+struct ci_scm_ts_pktinfo {
+  ci_uint32 if_index;
+  ci_uint32 pkt_length;
+  ci_uint32 reserved[2];
+};
+
 /* Indicates whether we want TX NIC timestamping, regardless of whether
  * SO_TIMESTAMPING has been overridden for onload timestamps */
 static inline int /*bool*/

diff --git a/src/include/cplane/cplane.h b/src/include/cplane/cplane.h
@@ -289,6 +289,16 @@ oo_cp_llap_params_check_logical(cicp_llap_row_t* llap,
          ! (llap->encap.type & CICP_LLAP_TYPE_SLAVE);
 }
 
+static inline int/*bool*/
+oo_cp_llap_params_check_physical(cicp_llap_row_t* llap,
+                                 ci_hwport_id_t hwport, ci_uint16 vlan_id,
+                                 const uint8_t* mac)
+{
+  return oo_cp_llap_params_check_hwport_mac(llap, hwport, mac) &&
+         (llap->encap.type == CICP_LLAP_TYPE_SLAVE ||
+          llap->encap.type == CICP_LLAP_TYPE_NONE);
+}
+
 typedef int (*oo_cp_llap_params_check)(cicp_llap_row_t *llap,
                                       ci_hwport_id_t hwport, ci_uint16 vlan_id,
                                       const uint8_t *mac);

diff --git a/src/lib/transport/ip/common_sockopts.c b/src/lib/transport/ip/common_sockopts.c
@@ -1731,7 +1731,6 @@ int ci_set_sol_socket(ci_netif* netif, ci_sock_cmn* s,
       goto fail_inval;
     /* We don't currently support any of these */
     if( v & (ONLOAD_SOF_TIMESTAMPING_OPT_STATS |
-             ONLOAD_SOF_TIMESTAMPING_OPT_PKTINFO |
              ONLOAD_SOF_TIMESTAMPING_OPT_TX_SWHW |
              ONLOAD_SOF_TIMESTAMPING_BIND_PHC) )
       goto fail_inval;

diff --git a/src/lib/transport/ip/ip_cmsg.c b/src/lib/transport/ip/ip_cmsg.c
@@ -109,11 +109,13 @@ llap_vs_pktinfo(struct oo_cplane_handle* cp,
   return 0;
 }
 
-ci_ifid_t ci_rx_pkt_ifindex(ci_netif* ni, const ci_ip_pkt_fmt* pkt)
+ci_ifid_t ci_rx_pkt_ifindex(ci_netif* ni, const ci_ip_pkt_fmt* pkt,
+                            bool phys_intf)
 {
   int ifindex = 0;
   struct llap_data l = {
-    .check = oo_cp_llap_params_check_logical,
+    .check = phys_intf ? oo_cp_llap_params_check_physical
+                       : oo_cp_llap_params_check_logical,
     .hwport = ni->state->intf_i_to_hwport[pkt->intf_i],
     .vlan_id = pkt->vlan,
     .mac = oo_ether_hdr_const(pkt)->ether_dhost,
@@ -148,9 +150,9 @@ ci_ifid_t ci_rx_pkt_ifindex(ci_netif* ni, const ci_ip_pkt_fmt* pkt)
   ifindex = oo_cp_hwport_vlan_to_ifindex(ni->cplane, l.check,
                                          l.hwport, l.vlan_id, l.mac);
   if( ifindex <= 0 ) {
-    LOG_E(ci_log("%s: oo_cp_hwport_vlan_to_ifindex(intf_i=%d => hwport=%d, "
-                 "vlan_id=%d mac="CI_MAC_PRINTF_FORMAT" ) failed",
-                 __FUNCTION__, pkt->intf_i, l.hwport, l.vlan_id,
+    LOG_E(ci_log("%s: oo_cp_hwport_vlan_to_ifindex(intf_i=%d => physical=%d, "
+                 "hwport=%d, vlan_id=%d mac="CI_MAC_PRINTF_FORMAT" ) failed",
+                 __FUNCTION__, !!phys_intf, pkt->intf_i, l.hwport, l.vlan_id,
                  CI_MAC_PRINTF_ARGS(oo_ether_hdr_const(pkt)->ether_dhost)));
   }
 
@@ -189,7 +191,7 @@ static void ip_cmsg_recv_pktinfo(ci_netif* netif, ci_udp_state* us,
     return;
   }
 
-  ifindex = ci_rx_pkt_ifindex(netif, pkt);
+  ifindex = ci_rx_pkt_ifindex(netif, pkt, 0);
 
   if( af_info == AF_INET ) {
     struct in_pktinfo info;
@@ -365,6 +367,17 @@ void ip_cmsg_recv_timestamping(ci_netif *ni, const ci_ip_pkt_fmt *pkt,
 
     ci_put_cmsg(cmsg_state, SOL_SOCKET, ONLOAD_SO_TIMESTAMPING, sizeof(ts), &ts);
   }
+
+  if( flags & ONLOAD_SOF_TIMESTAMPING_OPT_PKTINFO ) {
+    struct ci_scm_ts_pktinfo ts_pktinfo;
+
+    ts_pktinfo.if_index = ci_rx_pkt_ifindex(ni, pkt, 1);
+    /* Length at L2 = L2 header length + length reported at IP level */
+    ts_pktinfo.pkt_length = oo_pre_l3_len(pkt) + RX_PKT_PAYLOAD_LEN(pkt);
+
+    ci_put_cmsg(cmsg_state, SOL_SOCKET, ONLOAD_SCM_TIMESTAMPING_PKTINFO,
+                sizeof(ts_pktinfo), &ts_pktinfo);
+  }
 }
 #endif
 

diff --git a/src/lib/transport/ip/ip_internal.h b/src/lib/transport/ip/ip_internal.h
@@ -408,6 +408,7 @@ ci_tcp_ep_mcast_add_del(ci_netif*         ni,
  */
 #define ONLOAD_SO_TIMESTAMPING 37
 #define ONLOAD_SCM_TIMESTAMPING ONLOAD_SO_TIMESTAMPING
+#define ONLOAD_SCM_TIMESTAMPING_PKTINFO 58
 #endif
 
 /* Replica of sock_extended_err - just in case we do not have ee_data in