[RFC 04/13] TDLS: process discovery requests and send discovery responses

[Arik Nemtsov]

When a discovery request is received, add the peer to the TDLS peer
cache and send a response containing minimal data. Mandatory IEs in
the discovery response frame will be filled out by the driver.

Signed-off-by: Arik Nemtsov <arik at wizery.com>
Cc: Kalyan C Gaddam <chakkal at iit.edu>
---
 src/common/ieee802_11_defs.h |    1 +
 src/rsn_supp/tdls.c          |   89 +++++++++++++++++++++++++++++++++++++++++-
 2 files changed, 89 insertions(+), 1 deletions(-)

diff --git a/src/common/ieee802_11_defs.h b/src/common/ieee802_11_defs.h
index 86868c0..7db14f3 100644
--- a/src/common/ieee802_11_defs.h
+++ b/src/common/ieee802_11_defs.h
@@ -249,6 +249,7 @@
 #define WLAN_PA_GAS_INITIAL_RESP 11
 #define WLAN_PA_GAS_COMEBACK_REQ 12
 #define WLAN_PA_GAS_COMEBACK_RESP 13
+#define WLAN_TDLS_DISCOVERY_RESPONSE 14
 
 /* SA Query Action frame (IEEE 802.11w/D8.0, 7.4.9) */
 #define WLAN_SA_QUERY_REQUEST 0
diff --git a/src/rsn_supp/tdls.c b/src/rsn_supp/tdls.c
index e751867..49c4c4c 100644
--- a/src/rsn_supp/tdls.c
+++ b/src/rsn_supp/tdls.c
@@ -214,7 +214,9 @@ static int wpa_tdls_tpk_send(struct wpa_sm *sm, const u8 *dest, u8 action_code,
 	}
 
 	if (action_code == WLAN_TDLS_SETUP_CONFIRM ||
-	    action_code == WLAN_TDLS_TEARDOWN)
+	    action_code == WLAN_TDLS_TEARDOWN ||
+	    action_code == WLAN_TDLS_DISCOVERY_REQUEST ||
+	    action_code == WLAN_TDLS_DISCOVERY_RESPONSE)
 		return 0; /* No retries */
 
 	for (peer = sm->tdls; peer; peer = peer->next) {
@@ -802,6 +804,27 @@ static int wpa_tdls_send_error(struct wpa_sm *sm, const u8 *dst,
 }
 
 
+static struct wpa_tdls_peer *
+wpa_tdls_add_peer(struct wpa_sm *sm, const u8* addr)
+{
+	struct wpa_tdls_peer *peer;
+
+	wpa_printf(MSG_INFO, "TDLS: creating peer entry for " MACSTR,
+		   MAC2STR(addr));
+
+	peer = os_malloc(sizeof(*peer));
+	if (peer == NULL)
+		return NULL;
+
+	os_memset(peer, 0, sizeof(*peer));
+	os_memcpy(peer->addr, addr, ETH_ALEN);
+	peer->next = sm->tdls;
+	sm->tdls = peer;
+
+	return peer;
+}
+
+
 static int wpa_tdls_send_tpk_m1(struct wpa_sm *sm,
 				struct wpa_tdls_peer *peer)
 {
@@ -1143,6 +1166,67 @@ skip_ies:
 	return 0;
 }
 
+static int wpa_tdls_send_discovery_response(
+	struct wpa_sm *sm, struct wpa_tdls_peer *peer, u8 dialog_token)
+{
+	wpa_printf(MSG_DEBUG, "TDLS: Sending TDLS Discovery Response "
+			"(peer " MACSTR ")",  MAC2STR(peer->addr));
+
+	return wpa_tdls_tpk_send(sm, peer->addr, WLAN_TDLS_DISCOVERY_RESPONSE,
+				 dialog_token, 0, NULL, 0);
+}
+
+
+static int
+wpa_tdls_process_discovery_request(struct wpa_sm *sm, const u8 *addr,
+				   const u8 *buf, size_t len)
+{
+	struct wpa_eapol_ie_parse kde;
+	struct wpa_tdls_lnkid *lnkid;
+	struct wpa_tdls_peer *peer;
+	size_t min_req_len = sizeof(struct wpa_tdls_frame) +
+			     1 + /* dialog token */
+			     sizeof(struct wpa_tdls_lnkid);
+	u8 dialog_token;
+
+	wpa_printf(MSG_DEBUG, "TDLS discovery req from " MACSTR,
+		   MAC2STR(addr));
+
+	if (len < min_req_len) {
+		wpa_printf(MSG_DEBUG, "TDLS Discovery req is too short: %d",
+			   len);
+		return -1;
+	}
+
+	dialog_token = buf[sizeof(struct wpa_tdls_frame)];
+
+	if (wpa_supplicant_parse_ies(buf + sizeof(struct wpa_tdls_frame) + 1,
+				     len - (sizeof(struct wpa_tdls_frame) + 1),
+				     &kde) < 0)
+		return -1;
+
+	if (!kde.lnkid) {
+		wpa_printf(MSG_DEBUG, "TDLS: link ID not found in discovery "
+			   "request");
+		return -1;
+	}
+
+	lnkid = (void *)kde.lnkid;
+
+	if (os_memcmp(sm->bssid, lnkid->bssid, ETH_ALEN) != 0) {
+		wpa_printf(MSG_DEBUG, "TDLS: Discovery request from different "
+			   " BSS " MACSTR, MAC2STR(lnkid->bssid));
+		return -1;
+	}
+
+	peer = wpa_tdls_add_peer(sm, addr);
+	if (peer == NULL)
+		return -1;
+
+	return wpa_tdls_send_discovery_response(sm, peer, dialog_token);
+}
+
+
 
 static int wpa_tdls_process_tpk_m1(struct wpa_sm *sm, const u8 *src_addr,
 				   const u8 *buf, size_t len)
@@ -1937,6 +2021,9 @@ static void wpa_supplicant_rx_tdls(void *ctx, const u8 *src_addr,
 	case WLAN_TDLS_TEARDOWN:
 		wpa_tdls_recv_teardown(sm, src_addr, buf, len);
 		break;
+	case WLAN_TDLS_DISCOVERY_REQUEST:
+		wpa_tdls_process_discovery_request(sm, src_addr, buf, len);
+		break;
 	default:
 		/* Kernel code will process remaining frames */
 		wpa_printf(MSG_DEBUG, "TDLS: Ignore TDLS frame action code %u",
-- 
1.7.4.1