[ovs-dev,2/6] userspace: enable set_field support for nsh fields

diff --git a/lib/odp-execute.c b/lib/odp-execute.c
index 4eb5f40..30385f6 100644
--- a/lib/odp-execute.c
+++ b/lib/odp-execute.c
@@ -270,6 +270,66 @@  odp_set_nd(struct dp_packet *packet, const struct ovs_key_nd *key,
     }
 }
 
+/* Set the NSH header. Assumes the NSH header is present and matches the
+ * MD format of the key. The slow path must take case of that. */
+static void
+odp_set_nsh(struct dp_packet *packet, const struct ovs_key_nsh *key,
+            const struct ovs_key_nsh *mask)
+{
+    struct nsh_hdr * nsh = (struct nsh_hdr *) dp_packet_l3(packet);
+    uint16_t *p, *k, *m;
+    size_t len;
+    ovs_be32 path_hdr;
+    uint8_t flags;
+    int i;
+
+    ovs_assert(nsh != NULL);
+
+    if (!mask) {
+        nsh->ver_flags_len = htons(key->flags << NSH_FLAGS_SHIFT) |
+                             (nsh->ver_flags_len & ~htons(NSH_FLAGS_MASK));
+        put_16aligned_be32((ovs_16aligned_be32 *) &nsh->path_hdr,
+                           key->path_hdr);
+        switch (nsh->md_type) {
+            case NSH_M_TYPE1:
+                /* Avoid the 16/32 bit alignment hassle. */
+                memcpy(&nsh->md1, &key->c1, sizeof(struct nsh_md1_ctx));
+                break;
+            case NSH_M_TYPE2:
+                /* TODO */
+                break;
+            default:
+                OVS_NOT_REACHED();
+        }
+    } else {
+        flags = (ntohs(nsh->ver_flags_len) & NSH_FLAGS_MASK) >> NSH_FLAGS_SHIFT;
+        flags = key->flags | (flags & ~mask->flags);
+        nsh->ver_flags_len = htons(flags << NSH_FLAGS_SHIFT) |
+                             (nsh->ver_flags_len & ~htons(NSH_FLAGS_MASK));
+        path_hdr = get_16aligned_be32((ovs_16aligned_be32 *) &nsh->path_hdr);
+        path_hdr = key->path_hdr | (path_hdr & ~mask->path_hdr);
+        put_16aligned_be32((ovs_16aligned_be32 *) &nsh->path_hdr,
+                           path_hdr);
+        switch (nsh->md_type) {
+            case NSH_M_TYPE1:
+                len = sizeof(struct nsh_md1_ctx) >> 1;
+                /* Avoid the 16/32 bit alignment hassle. */
+                p = (uint16_t *) &nsh->md1.c1;
+                k = (uint16_t *) &key->c1;
+                m = (uint16_t *) &mask->c1;
+                for (i=0; i<len; i++, p++, k++, m++) {
+                    *p = *k | (*p & ~*m);
+                }
+                break;
+            case NSH_M_TYPE2:
+                /* TODO */
+                break;
+            default:
+                OVS_NOT_REACHED();
+        }
+    }
+}
+
 static void
 odp_execute_set_action(struct dp_packet *packet, const struct nlattr *a)
 {
@@ -296,7 +356,9 @@  odp_execute_set_action(struct dp_packet *packet, const struct nlattr *a)
         break;
 
     case OVS_KEY_ATTR_NSH:
+        odp_set_nsh(packet, nl_attr_get(a), NULL);
         break;
+
     case OVS_KEY_ATTR_IPV4:
         ipv4_key = nl_attr_get_unspec(a, sizeof(struct ovs_key_ipv4));
         packet_set_ipv4(packet, ipv4_key->ipv4_src,
@@ -422,7 +484,10 @@  odp_execute_masked_set_action(struct dp_packet *packet,
         break;
 
     case OVS_KEY_ATTR_NSH:
+        odp_set_nsh(packet, nl_attr_get(a),
+                    get_mask(a, struct ovs_key_nsh));
         break;
+
     case OVS_KEY_ATTR_IPV4:
         odp_set_ipv4(packet, nl_attr_get(a),
                      get_mask(a, struct ovs_key_ipv4));
diff --git a/lib/odp-util.c b/lib/odp-util.c
index b4427f0..14bdf96 100644
--- a/lib/odp-util.c
+++ b/lib/odp-util.c
@@ -6435,6 +6435,38 @@  put_nsh_key(const struct ovs_key_nsh *nsh, struct flow *flow, bool is_mask OVS_U
     }
 }
 
+static void
+commit_set_nsh_action(const struct flow *flow, struct flow *base_flow,
+                      struct ofpbuf *odp_actions,
+                      struct flow_wildcards *wc,
+                      bool use_masked)
+{
+    struct ovs_key_nsh key, mask, base;
+
+    if (flow->dl_type != htons(ETH_TYPE_NSH) ||
+        !memcmp(&base_flow->nsh, &flow->nsh, sizeof base_flow->nsh)) {
+        return;
+    }
+
+    /* Check that mdtype and np remain unchanged. */
+    ovs_assert(flow->nsh.mdtype == base_flow->nsh.mdtype &&
+               flow->nsh.np == base_flow->nsh.np);
+
+    get_nsh_key(flow, &key, false);
+    get_nsh_key(base_flow, &base, false);
+    get_nsh_key(&wc->masks, &mask, true);
+    mask.mdtype = 0;     /* Not writable. */
+    mask.np = 0;         /* Not writable. */
+
+    if (commit(OVS_KEY_ATTR_NSH, use_masked, &key, &base, &mask, sizeof key,
+               odp_actions)) {
+        put_nsh_key(&base, base_flow, false);
+        if (mask.mdtype != 0) { /* Mask was changed by commit(). */
+            put_nsh_key(&mask, &wc->masks, true);
+        }
+    }
+}
+
 /* TCP, UDP, and SCTP keys have the same layout. */
 BUILD_ASSERT_DECL(sizeof(struct ovs_key_tcp) == sizeof(struct ovs_key_udp) &&
                   sizeof(struct ovs_key_tcp) == sizeof(struct ovs_key_sctp));
@@ -6598,6 +6630,7 @@  commit_odp_actions(const struct flow *flow, struct flow *base,
         commit_mpls_action(flow, base, odp_actions);
         mpls_done = true;
     }
+    commit_set_nsh_action(flow, base, odp_actions, wc, use_masked);
     slow1 = commit_set_nw_action(flow, base, odp_actions, wc, use_masked);
     commit_set_port_action(flow, base, odp_actions, wc, use_masked);
     slow2 = commit_set_icmp_action(flow, base, odp_actions, wc);