cw1200: Start advanced filtering on system suspend

Enables advanced filtering when system is entering suspend to
avoid device-driven wakeups on frames the system can ignore in
suspend.

ST-Ericsson ID: 418353
ST-Ericsson FOSS-OUT ID: NA

Change-Id: I7d62c28e769448388b32bacc2d2e95dcd795c925
Signed-off-by: Dmitry Tarnyagin <dmitry.tarnyagin@stericsson.com>
Reviewed-on: http://gerrit.lud.stericsson.com/gerrit/49996
Reviewed-by: Filip MATUSIAK <filip.matusiak@tieto.com>
Tested-by: Filip MATUSIAK <filip.matusiak@tieto.com>
Reviewed-by: Bartosz MARKOWSKI <bartosz.markowski@tieto.com>

2 files changed, 132 insertions, 1 deletions

diff --git a/drivers/staging/cw1200/pm.c b/drivers/staging/cw1200/pm.c
index fc9c2354758..049703155db 100644
--- a/drivers/staging/cw1200/pm.c
+++ b/drivers/staging/cw1200/pm.c
@@ -10,6 +10,7 @@
  */
 
 #include <linux/platform_device.h>
+#include <linux/if_ether.h>
 #include "cw1200.h"
 #include "pm.h"
 #include "sta.h"
@@ -18,6 +19,67 @@
 
 #define CW1200_BEACON_SKIPPING_MULTIPLIER 3
 
+struct cw1200_udp_port_filter {
+	struct wsm_udp_port_filter_hdr hdr;
+	struct wsm_udp_port_filter dhcp;
+	struct wsm_udp_port_filter upnp;
+} __packed;
+
+struct cw1200_ether_type_filter {
+	struct wsm_ether_type_filter_hdr hdr;
+	struct wsm_ether_type_filter ip;
+	struct wsm_ether_type_filter pae;
+	struct wsm_ether_type_filter wapi;
+} __packed;
+
+static struct cw1200_udp_port_filter cw1200_udp_port_filter_on = {
+	.hdr.nrFilters = 2,
+	.dhcp = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_OUT,
+		.portType = WSM_FILTER_PORT_TYPE_DST,
+		.udpPort = __cpu_to_le16(67),
+	},
+	.upnp = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_OUT,
+		.portType = WSM_FILTER_PORT_TYPE_DST,
+		.udpPort = __cpu_to_le16(1900),
+	},
+	/* Please add other known ports to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+
+static struct wsm_udp_port_filter_hdr cw1200_udp_port_filter_off = {
+	.nrFilters = 0,
+};
+
+#ifndef ETH_P_WAPI
+#define ETH_P_WAPI     0x88B4
+#endif
+
+static struct cw1200_ether_type_filter cw1200_ether_type_filter_on = {
+	.hdr.nrFilters = 3,
+	.ip = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_IP),
+	},
+	.pae = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_PAE),
+	},
+	.wapi = {
+		.filterAction = WSM_FILTER_ACTION_FILTER_IN,
+		.etherType = __cpu_to_le16(ETH_P_WAPI),
+	},
+	/* Please add other known ether types to be filtered out here and
+	 * update nrFilters field in the header.
+	 * Up to 4 filters are allowed. */
+};
+
+static struct wsm_ether_type_filter_hdr cw1200_ether_type_filter_off = {
+	.nrFilters = 0,
+};
+
 static int cw1200_suspend_late(struct device *dev);
 static void cw1200_pm_release(struct device *dev);
 static int cw1200_pm_probe(struct platform_device *pdev);
@@ -236,7 +298,13 @@ int cw1200_wow_suspend(struct ieee80211_hw *hw, struct cfg80211_wowlan *wowlan)
 	/* Lock TX. */
 	wsm_lock_tx_async(priv);
 	if (priv->hw_bufs_used)
-		goto revert3;
+		goto revert2;
+
+	/* Set UDP filter */
+	wsm_set_udp_port_filter(priv, &cw1200_udp_port_filter_on.hdr);
+
+	/* Set ethernet frame type filter */
+	wsm_set_ether_type_filter(priv, &cw1200_ether_type_filter_on.hdr);
 
 	/* Allocate state */
 	state = kzalloc(sizeof(struct cw1200_suspend_state), GFP_KERNEL);
@@ -313,6 +381,9 @@ revert4:
 			state->link_id_gc);
 	kfree(state);
 revert3:
+	wsm_set_udp_port_filter(priv, &cw1200_udp_port_filter_off);
+	wsm_set_ether_type_filter(priv, &cw1200_ether_type_filter_off);
+revert2:
 	wsm_unlock_tx(priv);
 	up(&priv->scan.lock);
 revert1:
@@ -362,6 +433,12 @@ int cw1200_wow_resume(struct ieee80211_hw *hw)
 			jiffies + CW1200_BLOCK_ACK_INTERVAL);
 	spin_unlock_bh(&priv->ba_lock);
 
+	/* Remove UDP port filter */
+	wsm_set_udp_port_filter(priv, &cw1200_udp_port_filter_off);
+
+	/* Remove ethernet frame type filter */
+	wsm_set_ether_type_filter(priv, &cw1200_ether_type_filter_off);
+
 	/* Unlock datapath */
 	wsm_unlock_tx(priv);
 
diff --git a/drivers/staging/cw1200/wsm.h b/drivers/staging/cw1200/wsm.h
index 004a55f1240..4f7360d0bf3 100644
--- a/drivers/staging/cw1200/wsm.h
+++ b/drivers/staging/cw1200/wsm.h
@@ -572,6 +572,17 @@ struct cw1200_common;
 /* This is applicable only to Transmit */
 #define WSM_REQUEUE                     (11)
 
+/* Advanced filtering options */
+#define WSM_MAX_FILTER_ELEMENTS		(4)
+
+#define WSM_FILTER_ACTION_IGNORE	(0)
+#define WSM_FILTER_ACTION_FILTER_IN	(1)
+#define WSM_FILTER_ACTION_FILTER_OUT	(2)
+
+#define WSM_FILTER_PORT_TYPE_DST	(0)
+#define WSM_FILTER_PORT_TYPE_SRC	(1)
+
+
 
 struct wsm_hdr {
 	__le16 len;
@@ -1516,6 +1527,49 @@ static inline int wsm_set_tx_rate_retry_policy(struct cw1200_common *priv,
 			     size);
 }
 
+/* 4.32 SetEtherTypeDataFrameFilter */
+struct wsm_ether_type_filter_hdr {
+	u8 nrFilters;		/* Up to WSM_MAX_FILTER_ELEMENTS */
+	u8 reserved[3];
+} __packed;
+
+struct wsm_ether_type_filter {
+	u8 filterAction;	/* WSM_FILTER_ACTION_XXX */
+	u8 reserved;
+	__le16 etherType;	/* Type of ethernet frame */
+} __packed;
+
+static inline int wsm_set_ether_type_filter(struct cw1200_common *priv,
+				struct wsm_ether_type_filter_hdr *arg)
+{
+	size_t size = sizeof(struct wsm_ether_type_filter_hdr) +
+		arg->nrFilters * sizeof(struct wsm_ether_type_filter);
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_ETHERTYPE_DATAFRAME_FILTER,
+		arg, size);
+}
+
+
+/* 4.33 SetUDPPortDataFrameFilter */
+struct wsm_udp_port_filter_hdr {
+	u8 nrFilters;		/* Up to WSM_MAX_FILTER_ELEMENTS */
+	u8 reserved[3];
+} __packed;
+
+struct wsm_udp_port_filter {
+	u8 filterAction;	/* WSM_FILTER_ACTION_XXX */
+	u8 portType;		/* WSM_FILTER_PORT_TYPE_XXX */
+	__le16 udpPort;		/* Port number */
+} __packed;
+
+static inline int wsm_set_udp_port_filter(struct cw1200_common *priv,
+				struct wsm_udp_port_filter_hdr *arg)
+{
+	size_t size = sizeof(struct wsm_udp_port_filter_hdr) +
+		arg->nrFilters * sizeof(struct wsm_udp_port_filter);
+	return wsm_write_mib(priv, WSM_MIB_ID_SET_UDPPORT_DATAFRAME_FILTER,
+		arg, size);
+}
+
 /* Undocumented MIBs: */
 /* 4.35 P2PDeviceInfo */
 #define D11_MAX_SSID_LEN		(32)