datapath-windows: Accept MPLS feature probe.

Currently all feature probe messages sent from userspace are
suppressed by the OVS extension.

This patch changes the current behaviour to allow feature probe
for MPLS.

Signed-off-by: Sorin Vinturis <svinturis@cloudbasesolutions.com>
Acked-by: Nithin Raju <nithin@vmware.com>
Signed-off-by: Gurucharan Shetty <guru@ovn.org>

3 files changed, 92 insertions, 7 deletions

diff --git a/datapath-windows/ovsext/Flow.c b/datapath-windows/ovsext/Flow.c
index a2f053542..c22e8cfb2 100644
--- a/datapath-windows/ovsext/Flow.c
+++ b/datapath-windows/ovsext/Flow.c
@@ -86,6 +86,8 @@ static NTSTATUS _MapFlowMplsKeyToNlKey(PNL_BUFFER nlBuf,
 static NTSTATUS OvsDoDumpFlows(OvsFlowDumpInput *dumpInput,
                                OvsFlowDumpOutput *dumpOutput,
                                UINT32 *replyLen);
+static NTSTATUS OvsProbeSupportedFeature(POVS_MESSAGE msgIn,
+                                         PNL_ATTR keyAttr);
 
 
 #define OVS_FLOW_TABLE_SIZE 2048
@@ -313,13 +315,17 @@ OvsFlowNlCmdHandler(POVS_USER_PARAMS_CONTEXT usrParamsCtx,
     }
 
     if (flowAttrs[OVS_FLOW_ATTR_PROBE]) {
-        OVS_LOG_ERROR("Attribute OVS_FLOW_ATTR_PROBE not supported");
-        goto done;
+        rc = OvsProbeSupportedFeature(msgIn, flowAttrs[OVS_FLOW_ATTR_KEY]);
+        if (rc != STATUS_SUCCESS) {
+            nlError = NlMapStatusToNlErr(rc);
+            goto done;
+        }
     }
 
     if ((rc = _MapNlToFlowPut(msgIn, flowAttrs[OVS_FLOW_ATTR_KEY],
-        flowAttrs[OVS_FLOW_ATTR_ACTIONS], flowAttrs[OVS_FLOW_ATTR_CLEAR],
-         &mappedFlow))
+                              flowAttrs[OVS_FLOW_ATTR_ACTIONS],
+                              flowAttrs[OVS_FLOW_ATTR_CLEAR],
+                              &mappedFlow))
         != STATUS_SUCCESS) {
         OVS_LOG_ERROR("Conversion to OvsFlowPut failed");
         goto done;
@@ -1254,7 +1260,7 @@ _MapNlToFlowPut(POVS_MESSAGE msgIn, PNL_ATTR keyAttr,
                            keyAttrs, ARRAY_SIZE(keyAttrs)))
                            != TRUE) {
         OVS_LOG_ERROR("Key Attr Parsing failed for msg: %p",
-                       nlMsgHdr);
+                      nlMsgHdr);
         rc = STATUS_INVALID_PARAMETER;
         goto done;
     }
@@ -1272,7 +1278,7 @@ _MapNlToFlowPut(POVS_MESSAGE msgIn, PNL_ATTR keyAttr,
                                tunnelAttrs, ARRAY_SIZE(tunnelAttrs)))
                                != TRUE) {
             OVS_LOG_ERROR("Tunnel key Attr Parsing failed for msg: %p",
-                           nlMsgHdr);
+                          nlMsgHdr);
             rc = STATUS_INVALID_PARAMETER;
             goto done;
         }
@@ -1290,7 +1296,7 @@ _MapNlToFlowPut(POVS_MESSAGE msgIn, PNL_ATTR keyAttr,
     mappedFlow->dpNo = ovsHdr->dp_ifindex;
 
     _MapNlToFlowPutFlags(genlMsgHdr, flowAttrClear,
-                                mappedFlow);
+                         mappedFlow);
 
 done:
     return rc;
@@ -2521,4 +2527,56 @@ OvsTunKeyAttrSize(void)
          + NlAttrTotalSize(2);   /* OVS_TUNNEL_KEY_ATTR_TP_DST */
 }
 
+/*
+ *----------------------------------------------------------------------------
+ *  OvsProbeSupportedFeature --
+ *    Verifies if the probed feature is supported.
+ * 
+ * Results:
+ *   STATUS_SUCCESS if the probed feature is supported.
+ *----------------------------------------------------------------------------
+ */
+static NTSTATUS
+OvsProbeSupportedFeature(POVS_MESSAGE msgIn,
+                         PNL_ATTR keyAttr)
+{
+    NTSTATUS status = STATUS_SUCCESS;
+    PNL_MSG_HDR nlMsgHdr = &(msgIn->nlMsg);
+
+    UINT32 keyAttrOffset = (UINT32)((PCHAR)keyAttr - (PCHAR)nlMsgHdr);
+    PNL_ATTR keyAttrs[__OVS_KEY_ATTR_MAX] = { NULL };
+
+    /* Get flow keys attributes */
+    if ((NlAttrParseNested(nlMsgHdr, keyAttrOffset, NlAttrLen(keyAttr),
+                           nlFlowKeyPolicy, ARRAY_SIZE(nlFlowKeyPolicy),
+                           keyAttrs, ARRAY_SIZE(keyAttrs)))
+                           != TRUE) {
+        OVS_LOG_ERROR("Key Attr Parsing failed for msg: %p",
+                      nlMsgHdr);
+        status = STATUS_INVALID_PARAMETER;
+        goto done;
+    }
+
+    if (keyAttrs[OVS_KEY_ATTR_MPLS] &&
+        keyAttrs[OVS_KEY_ATTR_ETHERTYPE]) {
+        ovs_be16 ethType = NlAttrGetU16(keyAttrs[OVS_KEY_ATTR_ETHERTYPE]);
+
+        if (OvsEthertypeIsMpls(ethType)) {
+            if (!OvsCountMplsLabels(keyAttrs[OVS_KEY_ATTR_MPLS])) {
+                OVS_LOG_ERROR("Maximum supported MPLS labels exceeded.");
+                status = STATUS_INVALID_MESSAGE;
+            }
+        } else {
+            OVS_LOG_ERROR("Wrong ethertype for MPLS attribute.");
+            status = STATUS_INVALID_PARAMETER;
+        }
+    } else {
+        OVS_LOG_ERROR("Probed feature not supported.");
+        status = STATUS_INVALID_PARAMETER;
+    }
+
+done:
+    return status;
+}
+
 #pragma warning( pop )
diff --git a/datapath-windows/ovsext/Mpls.h b/datapath-windows/ovsext/Mpls.h
index 6e53e46db..084613f1a 100644
--- a/datapath-windows/ovsext/Mpls.h
+++ b/datapath-windows/ovsext/Mpls.h
@@ -53,4 +53,28 @@ OvsEthertypeIsMpls(ovs_be16 ethertype)
            ethertype == htons(ETH_TYPE_MPLS_MCAST);
 }
 
+/* Returns the number of MPLS LSEs present in 'a'
+ *
+ * Counts 'flow''s MPLS label stack entries (LESs) stopping at the first
+ * entry that has the bottom of stack (BOS) bit set. If no such entry exists,
+ * then zero is returned, meaning that the maximum number of supported
+ * MPLS LSEs exceeded.
+ */
+__inline UINT32
+OvsCountMplsLabels(PNL_ATTR a)
+{
+    const MPLSHdr *mpls = NlAttrGet(a);
+    UINT32 count = 0;
+    BOOLEAN bos = FALSE;
+
+    for (count = 0; count < FLOW_MAX_MPLS_LABELS; count++) {
+        if ((mpls + count)->lse & htonl(MPLS_BOS_MASK)) {
+            bos = TRUE;
+            break;
+        }
+    }
+
+    return bos ? ++count : 0;
+}
+
 #endif /* __MPLS_H_ */
diff --git a/datapath-windows/ovsext/Netlink/NetlinkError.h b/datapath-windows/ovsext/Netlink/NetlinkError.h
index eefa89e34..36115c802 100644
--- a/datapath-windows/ovsext/Netlink/NetlinkError.h
+++ b/datapath-windows/ovsext/Netlink/NetlinkError.h
@@ -229,6 +229,9 @@ NlMapStatusToNlErr(NTSTATUS status)
     case STATUS_OBJECT_NAME_EXISTS:
       ret = NL_ERROR_EXIST;
       break;
+    case STATUS_INVALID_MESSAGE:
+      ret = NL_ERROR_BADMSG;
+      break;
     default:
       ret = NL_ERROR_OTHER;
       break;