driver_nl80211: Add support for DRIVER RXFILTER command

Add support for this Android specific feature using WoWLAN packet
pattern triggers.

Signed-off-by: Pontus Fuchs <pontus.fuchs@tactel.se>

1 files changed, 174 insertions, 0 deletions

diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 80121ea..6f561fc 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -285,6 +285,10 @@ struct wpa_driver_nl80211_data {
 	int auth_wep_tx_keyidx;
 	int auth_local_state_change;
 	int auth_p2p;
+#ifdef ANDROID
+	u8 wowlan_triggers;
+	u8 wowlan_enabled;
+#endif
 };
 
 
@@ -8882,6 +8886,162 @@ static int nl80211_pmkid(struct i802_bss *bss, int cmd, const u8 *bssid,
 	return -ENOBUFS;
 }
 
+#define RX_SELF_FILTER			0
+#define RX_BROADCAST_FILTER		1
+#define RX_IPV4_MULTICAST_FILTER	2
+#define RX_IPV6_MULTICAST_FILTER	3
+#define NR_RX_FILTERS			4
+
+static const u8 filter_bcast[] = {0xff,0xff,0xff,0xff,0xff,0xff};
+static const u8 filter_ipv4mc[] = {0x01,0x00,0x5e};
+static const u8 filter_ipv6mc[] = {0x33,0x33};
+
+static int nl80211_get_wowlan_pat(struct i802_bss *bss, u8 *buf, int buflen,
+				  u8* mask, int filter)
+{
+	int len = 0, ret;
+	if (buflen < ETH_ALEN)
+		return -1;
+
+	switch(filter) {
+	case RX_SELF_FILTER:
+		ret = linux_get_ifhwaddr(bss->drv->ioctl_sock,
+					 bss->ifname, buf);
+		if (ret)
+			return -1;
+		len = ETH_ALEN;
+		*mask = 0x3F;
+		break;
+	case RX_BROADCAST_FILTER:
+		memcpy(buf, filter_bcast, sizeof(filter_bcast));
+		len = ETH_ALEN;
+		*mask = 0x3F;
+		break;
+	case RX_IPV4_MULTICAST_FILTER:
+		memcpy(buf, filter_ipv4mc, sizeof(filter_ipv4mc));
+		len = sizeof(filter_ipv4mc);
+		*mask = 0x7; /* 3 bytes */
+		break;
+	case RX_IPV6_MULTICAST_FILTER:
+		memcpy(buf, filter_ipv6mc, sizeof(filter_ipv6mc));
+		len = sizeof(filter_ipv6mc);
+		*mask = 0x3; /* 2 bytes */
+		break;
+	default:
+		len = -1;
+		break;
+	}
+	return len;
+}
+
+static int nl80211_set_wowlan_triggers(struct i802_bss *bss, int enable)
+{
+	struct nl_msg *msg, *pats = NULL;
+	struct nlattr *wowtrig, *pat;
+	int i, ret = -1;
+
+	bss->drv->wowlan_enabled = !!enable;
+
+	msg = nlmsg_alloc();
+	if (!msg)
+		return -ENOMEM;
+
+	genlmsg_put(msg, 0, 0, bss->drv->global->nl80211_id, 0,
+		    0, NL80211_CMD_SET_WOWLAN, 0);
+
+	NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, bss->drv->first_bss.ifindex);
+	wowtrig = nla_nest_start(msg, NL80211_ATTR_WOWLAN_TRIGGERS);
+
+	if (!wowtrig) {
+		ret = -ENOBUFS;
+		goto nla_put_failure;
+	}
+
+	if (!enable) {
+		NLA_PUT_FLAG(msg, NL80211_WOWLAN_TRIG_ANY);
+	} else {
+		pats = nlmsg_alloc();
+		if (!pats) {
+			ret = -ENOMEM;
+			goto nla_put_failure;
+		}
+
+		for (i = 0; i < NR_RX_FILTERS; i++) {
+			if (bss->drv->wowlan_triggers & (1 << i)) {
+				u8 patbuf[ETH_ALEN], patmask;
+				int patlen;
+				int patnr = 1;
+				int j;
+
+				patlen = nl80211_get_wowlan_pat(bss, patbuf,
+						      sizeof(patbuf),
+				                      &patmask, i);
+				if (!patlen)
+					continue;
+				else if (patlen < 0) {
+					ret = -1;
+					break;
+				}
+				pat = nla_nest_start(pats, patnr++);
+				NLA_PUT(pats, NL80211_WOWLAN_PKTPAT_MASK,
+						  1, &patmask);
+				NLA_PUT(pats, NL80211_WOWLAN_PKTPAT_PATTERN,
+						   patlen, patbuf);
+				nla_nest_end(pats, pat);
+			}
+		}
+	}
+
+	if (pats)
+		nla_put_nested(msg, NL80211_WOWLAN_TRIG_PKT_PATTERN, pats);
+
+	nla_nest_end(msg, wowtrig);
+
+	ret = send_and_recv_msgs(bss->drv, msg, NULL, NULL);
+
+	if (ret < 0)
+		wpa_printf(MSG_ERROR, "Failed to set WoWLAN trigger:%d\n", ret);
+
+	if (pats)
+		nlmsg_free(pats);
+
+	return 0;
+
+nla_put_failure:
+	nlmsg_free(msg);
+	return ret;
+}
+
+static int nl80211_toggle_wowlan_trigger(struct i802_bss *bss, int nr,
+					 int enabled)
+{
+	if (nr >= NR_RX_FILTERS) {
+		wpa_printf(MSG_ERROR, "Unknown filter: %d\n", nr);
+		return -1;
+	}
+
+	if (enabled)
+		bss->drv->wowlan_triggers |= 1 << nr;
+	else
+		bss->drv->wowlan_triggers &= ~(1 << nr);
+
+	if (bss->drv->wowlan_enabled)
+		nl80211_set_wowlan_triggers(bss, 1);
+
+	return 0;
+}
+
+static int nl80211_parse_wowlan_trigger_nr(char *s)
+{
+	long i;
+	char *endp;
+
+	i = strtol(s, &endp, 10);
+
+	if(endp == s)
+		return -1;
+	return i;
+}
 
 static int nl80211_add_pmkid(void *priv, const u8 *bssid, const u8 *pmkid)
 {
@@ -9373,6 +9533,20 @@ static int wpa_driver_nl80211_driver_cmd(void *priv, char *cmd, char *buf,
 		if (!ret && (state != -1))
 			ret = os_snprintf(buf, buf_len, "POWERMODE = %d\n",
 					  state);
+	} else if( os_strncasecmp(cmd, "RXFILTER-ADD ", 13) == 0 ) {
+		int i = nl80211_parse_wowlan_trigger_nr(cmd + 13);
+		if(i < 0)
+			return i;
+		return nl80211_toggle_wowlan_trigger(bss, i, 1);
+	} else if( os_strncasecmp(cmd, "RXFILTER-REMOVE ", 16) == 0 ) {
+		int i = nl80211_parse_wowlan_trigger_nr(cmd + 16);
+		if(i < 0)
+			return i;
+		return nl80211_toggle_wowlan_trigger(bss, i, 0);
+	} else if( os_strcasecmp(cmd, "RXFILTER-START") == 0 ) {
+		return nl80211_set_wowlan_triggers(bss, 1);
+	} else if( os_strcasecmp(cmd, "RXFILTER-STOP") == 0 ) {
+		return nl80211_set_wowlan_triggers(bss, 0);
 	} else { /* Use private command */
 		memset(&ifr, 0, sizeof(ifr));
 		memset(&priv_cmd, 0, sizeof(priv_cmd));