[Linuxptp-devel] [PATCH 2/4] RAW UDPv4: Add untagged traffic filter

[Andriy Kohut]

This is needed to support ports in access VLAN mode. To be used when the VLAN
option has a default value.

Signed-off-by: Andriy Kohut <andr...@mellanox.com>
---
 raw_udp.c | 63 +++++++++++++++++++++++++++++++++++++++++++++++++--------------
 1 file changed, 49 insertions(+), 14 deletions(-)

diff --git a/raw_udp.c b/raw_udp.c
index 6a6c550..08af003 100644
--- a/raw_udp.c
+++ b/raw_udp.c
@@ -108,9 +108,11 @@ struct raw_udp {
 #define OFF_DPORT_REL 16
 #define IP_FRAG_PART  0x1FFF
 
+#define N_UNTAGGED_FILTER 11
 #define N_TAGGED_FILTER   25
 
 #define VLAN_TEST          3
+#define UNTAGGED_PORT_TEST 8
 #define TAGGED_PORT_TEST   22
 
 enum { MC_PRIMARY, MC_PDELAY };
@@ -148,6 +150,20 @@ static struct sock_filter tagged_filter[N_TAGGED_FILTER] = 
{
        { OP_RETK,   0,  0, 0000000000      }, /*reject*/
 };
 
+static struct sock_filter untagged_filter[N_UNTAGGED_FILTER] = {
+       { OP_LDH,   0,   0, OFF_ETYPE     },
+       { OP_JEQ,   0,   8, ETH_P_IP      }, /*f goto reject*/
+       { OP_LDB,   0,   0, OFF_IP_PROTO  },
+       { OP_JEQ,   0,   6, IPPROTO_UDP   }, /*f goto reject*/
+       { OP_LDH,   0,   0, OFF_IP_FRAG   },
+       { OP_JSET,  4,   0, IP_FRAG_PART  }, /*t goto reject*/
+       { OP_LDXB,  0,   0, OFF_IP_HLEN   },
+       { OP_LDHIN, 0,   0, OFF_DPORT_REL },
+       { OP_JEQ,   0,   1, GENERAL_PORT  },
+       { OP_RETK,  0,   0, 1500          },
+       { OP_RETK,  0,   0, 0             },
+};
+
 static int raw_udp_configure(int fd, int ev, int index, unsigned char *addr1,
                             unsigned char *addr2, int op, unsigned int vlan)
 {
@@ -155,11 +171,18 @@ static int raw_udp_configure(int fd, int ev, int index, 
unsigned char *addr1,
        struct packet_mreq mreq;
        struct sock_fprog prg;
 
-       tagged_filter[VLAN_TEST].k = vlan;
-       tagged_filter[TAGGED_PORT_TEST].k = ev ? EVENT_PORT :
-                                                GENERAL_PORT;
-       prg.len = N_TAGGED_FILTER;
-       prg.filter = tagged_filter;
+       if (vlan != DEFAULT_VLAN) {
+               tagged_filter[VLAN_TEST].k = vlan;
+               tagged_filter[TAGGED_PORT_TEST].k = ev ? EVENT_PORT :
+                                                        GENERAL_PORT;
+               prg.len = N_TAGGED_FILTER;
+               prg.filter = tagged_filter;
+       } else {
+               untagged_filter[UNTAGGED_PORT_TEST].k = ev ? EVENT_PORT :
+                                                            GENERAL_PORT;
+               prg.len = N_UNTAGGED_FILTER;
+               prg.filter = untagged_filter;
+       }
 
        if (setsockopt(fd, SOL_SOCKET, SO_ATTACH_FILTER, &prg, sizeof(prg))) {
                pr_err("setsockopt SO_ATTACH_FILTER failed: %m");
@@ -427,6 +450,7 @@ static void raw_udp_packet_construct(struct raw_udp *raw, 
struct address *addr,
                                     enum transport_event event, int peer,
                                     int *len, unsigned char **ptr) {
        struct vlan_hdr *vlan_hdr;
+       struct eth_hdr  *eth_hdr;
        struct ip_hdr  *ip_hdr;
        struct udp_hdr *udp_hdr;
 
@@ -448,6 +472,7 @@ static void raw_udp_packet_construct(struct raw_udp *raw, 
struct address *addr,
        ip_hdr->ip_dst.s_addr = mcast_addr[MC_PRIMARY].s_addr;
        ip_hdr->ip_ttl = 128;
 
+       ip_hdr->ip_src = raw->ip_src.sin.sin_addr;
        udp_hdr->udp_check = 0;
        udp_hdr->udp_check = udp_csum(ip_hdr, *ptr + sizeof(*ip_hdr), *len);
 
@@ -466,15 +491,25 @@ static void raw_udp_packet_construct(struct raw_udp *raw, 
struct address *addr,
                addr = peer ? &raw->p2p_addr : &raw->ptp_addr;
 
        /* Ethernet VLAN Header */
-       *ptr -= sizeof(*vlan_hdr);
-       *len += sizeof(*vlan_hdr);
-       vlan_hdr = (struct vlan_hdr *) *ptr;
-
-       addr_to_mac(&vlan_hdr->dst, addr);
-       addr_to_mac(&vlan_hdr->src, &raw->src_addr);
-       vlan_hdr->tpid = htons(ETH_P_8021Q);
-       vlan_hdr->tci = htons(raw->vlan);
-       vlan_hdr->type = htons(ETH_P_IP);
+       if (raw->vlan != DEFAULT_VLAN) {
+               *ptr -= sizeof(*vlan_hdr);
+               *len += sizeof(*vlan_hdr);
+               vlan_hdr = (struct vlan_hdr *) *ptr;
+
+               addr_to_mac(&vlan_hdr->dst, addr);
+               addr_to_mac(&vlan_hdr->src, &raw->src_addr);
+               vlan_hdr->tpid = htons(ETH_P_8021Q);
+               vlan_hdr->tci = htons(raw->vlan);
+               vlan_hdr->type = htons(ETH_P_IP);
+       } else {
+               *ptr -= sizeof(*eth_hdr);
+               *len += sizeof(*eth_hdr);
+               eth_hdr = (struct eth_hdr *) *ptr;
+
+               addr_to_mac(&eth_hdr->dst, addr);
+               addr_to_mac(&eth_hdr->src, &raw->src_addr);
+               eth_hdr->type = htons(ETH_P_IP);
+       }
 }
 
 static int raw_udp_send(struct transport *t, struct fdarray *fda,
-- 
2.8.4


------------------------------------------------------------------------------
Check out the vibrant tech community on one of the world's most
engaging tech sites, Slashdot.org! http://sdm.link/slashdot
_______________________________________________
Linuxptp-devel mailing list
Linuxptp-devel@lists.sourceforge.net
https://lists.sourceforge.net/lists/listinfo/linuxptp-devel