[09/25] OCV: insert OCI in 4-way and group key handshake

diff --git a/src/ap/wpa_auth.c b/src/ap/wpa_auth.c
index f20dd806a..ea966474f 100644
--- a/src/ap/wpa_auth.c
+++ b/src/ap/wpa_auth.c
@@ -13,6 +13,7 @@ 
 #include "utils/state_machine.h"
 #include "utils/bitfield.h"
 #include "common/ieee802_11_defs.h"
+#include "common/ocv.h"
 #include "crypto/aes.h"
 #include "crypto/aes_wrap.h"
 #include "crypto/aes_siv.h"
@@ -2891,6 +2892,45 @@  static u8 * ieee80211w_kde_add(struct wpa_state_machine *sm, u8 *pos)
 
 #endif /* CONFIG_IEEE80211W */
 
+#ifdef CONFIG_OCV
+
+static int ocv_oci_len(struct wpa_state_machine *sm)
+{
+	if (wpa_auth_uses_ocv(sm))
+		return OCV_OCI_KDE_LEN;
+	return 0;
+}
+
+static int ocv_oci_add(struct wpa_state_machine *sm, u8 **argpos)
+{
+	struct wpa_channel_info ci;
+
+	if (!wpa_auth_uses_ocv(sm))
+		return 0;
+
+	if (wpa_channel_info(sm->wpa_auth, &ci) != 0) {
+		wpa_printf(MSG_WARNING, "Failed to get channel info "
+			   "for OCI element");
+		return -1;
+	}
+
+	return ocv_insert_oci_kde(&ci, argpos);
+}
+
+#else /* CONFIG_OCV */
+
+static int ocv_oci_len(struct wpa_state_machine *sm)
+{
+	return 0;
+}
+
+static int ocv_oci_add(struct wpa_state_machine *sm, u8 **pos)
+{
+	return 0;
+}
+
+#endif /* CONFIG_OCV */
+
 
 SM_STATE(WPA_PTK, PTKINITNEGOTIATING)
 {
@@ -2975,7 +3015,7 @@  SM_STATE(WPA_PTK, PTKINITNEGOTIATING)
 		}
 	}
 
-	kde_len = wpa_ie_len + ieee80211w_kde_len(sm);
+	kde_len = wpa_ie_len + ieee80211w_kde_len(sm) + ocv_oci_len(sm);
 	if (gtk)
 		kde_len += 2 + RSN_SELECTOR_LEN + 2 + gtk_len;
 #ifdef CONFIG_IEEE80211R_AP
@@ -3020,6 +3060,10 @@  SM_STATE(WPA_PTK, PTKINITNEGOTIATING)
 				  gtk, gtk_len);
 	}
 	pos = ieee80211w_kde_add(sm, pos);
+	if (ocv_oci_add(sm, &pos) < 0) {
+		os_free(kde);
+		return;
+	}
 
 #ifdef CONFIG_IEEE80211R_AP
 	if (wpa_key_mgmt_ft(sm->wpa_key_mgmt)) {
@@ -3331,7 +3375,7 @@  SM_STATE(WPA_PTK_GROUP, REKEYNEGOTIATING)
 	}
 	if (sm->wpa == WPA_VERSION_WPA2) {
 		kde_len = 2 + RSN_SELECTOR_LEN + 2 + gsm->GTK_len +
-			ieee80211w_kde_len(sm);
+			ieee80211w_kde_len(sm) + ocv_oci_len(sm);
 		kde_buf = os_malloc(kde_len);
 		if (kde_buf == NULL)
 			return;
@@ -3342,6 +3386,10 @@  SM_STATE(WPA_PTK_GROUP, REKEYNEGOTIATING)
 		pos = wpa_add_kde(pos, RSN_KEY_DATA_GROUPKEY, hdr, 2,
 				  gtk, gsm->GTK_len);
 		pos = ieee80211w_kde_add(sm, pos);
+		if (ocv_oci_add(sm, &pos) < 0) {
+			os_free(kde_buf);
+			return;
+		}
 		kde_len = pos - kde;
 	} else {
 		kde = gtk;
@@ -4675,7 +4723,7 @@  int wpa_auth_resend_m3(struct wpa_state_machine *sm,
 		}
 	}
 
-	kde_len = wpa_ie_len + ieee80211w_kde_len(sm);
+	kde_len = wpa_ie_len + ieee80211w_kde_len(sm) + ocv_oci_len(sm);
 	if (gtk)
 		kde_len += 2 + RSN_SELECTOR_LEN + 2 + gtk_len;
 #ifdef CONFIG_IEEE80211R_AP
@@ -4724,6 +4772,10 @@  int wpa_auth_resend_m3(struct wpa_state_machine *sm,
 		os_memset(opos, 0, 6); /* clear PN */
 	}
 #endif /* CONFIG_IEEE80211W */
+	if (ocv_oci_add(sm, &pos) < 0) {
+		os_free(kde);
+		return -1;
+	}
 
 #ifdef CONFIG_IEEE80211R_AP
 	if (wpa_key_mgmt_ft(sm->wpa_key_mgmt)) {
@@ -4805,7 +4857,7 @@  int wpa_auth_resend_group_m1(struct wpa_state_machine *sm,
 	gtk = gsm->GTK[gsm->GN - 1];
 	if (sm->wpa == WPA_VERSION_WPA2) {
 		kde_len = 2 + RSN_SELECTOR_LEN + 2 + gsm->GTK_len +
-			ieee80211w_kde_len(sm);
+			ieee80211w_kde_len(sm) + ocv_oci_len(sm);
 		kde_buf = os_malloc(kde_len);
 		if (kde_buf == NULL)
 			return -1;
@@ -4825,6 +4877,10 @@  int wpa_auth_resend_group_m1(struct wpa_state_machine *sm,
 			os_memset(opos, 0, 6); /* clear PN */
 		}
 #endif /* CONFIG_IEEE80211W */
+		if (ocv_oci_add(sm, &pos) < 0) {
+			os_free(kde_buf);
+			return -1;
+		}
 		kde_len = pos - kde;
 	} else {
 		kde = gtk;
diff --git a/src/rsn_supp/wpa.c b/src/rsn_supp/wpa.c
index 774ddd9e4..88437291b 100644
--- a/src/rsn_supp/wpa.c
+++ b/src/rsn_supp/wpa.c
@@ -20,8 +20,10 @@ 
 #include "crypto/sha512.h"
 #include "common/ieee802_11_defs.h"
 #include "common/ieee802_11_common.h"
+#include "common/ocv.h"
 #include "eap_common/eap_defs.h"
 #include "eapol_supp/eapol_supp_sm.h"
+#include "drivers/driver.h"
 #include "wpa.h"
 #include "eloop.h"
 #include "preauth.h"
@@ -543,7 +545,6 @@  static int wpa_derive_ptk(struct wpa_sm *sm, const unsigned char *src_addr,
 			      sm->pairwise_cipher);
 }
 
-
 static void wpa_supplicant_process_1_of_4(struct wpa_sm *sm,
 					  const unsigned char *src_addr,
 					  const struct wpa_eapol_key *key,
@@ -618,6 +619,33 @@  static void wpa_supplicant_process_1_of_4(struct wpa_sm *sm,
 	kde = sm->assoc_wpa_ie;
 	kde_len = sm->assoc_wpa_ie_len;
 
+#ifdef CONFIG_OCV
+	if (wpa_sm_ocv_enabled(sm)) {
+		struct wpa_channel_info ci;
+		u8 *pos;
+
+		if (wpa_sm_channel_info(sm, &ci) != 0) {
+			wpa_printf(MSG_WARNING, "Failed to get channel info "
+				   "for OCI element in EAPOL-Key 2/4");
+			goto failed;
+		}
+
+		kde_buf = os_malloc(kde_len + 2 + RSN_SELECTOR_LEN + 3);
+		if (!kde_buf) {
+			wpa_printf(MSG_WARNING, "Failed to allocate memory for "
+				   "KDE with OCI in EAPOL-Key 2/4");
+			goto failed;
+		}
+
+		os_memcpy(kde_buf, kde, kde_len);
+		kde = kde_buf;
+		pos = kde + kde_len;
+		if (ocv_insert_oci_kde(&ci, &pos) < 0)
+			goto failed;
+		kde_len = pos - kde;
+	}
+#endif /* CONFIG_OCV */
+
 #ifdef CONFIG_P2P
 	if (sm->p2p) {
 		kde_buf = os_malloc(kde_len + 2 + RSN_SELECTOR_LEN + 1);
@@ -1625,11 +1653,17 @@  static int wpa_supplicant_send_2_of_2(struct wpa_sm *sm,
 	size_t mic_len, hdrlen, rlen;
 	struct wpa_eapol_key *reply;
 	u8 *rbuf, *key_mic;
+	size_t kde_len = 0;
+
+#ifdef CONFIG_OCV
+	if (wpa_sm_ocv_enabled(sm))
+		kde_len = OCV_OCI_KDE_LEN;
+#endif /* CONFIG_OCV */
 
 	mic_len = wpa_mic_len(sm->key_mgmt, sm->pmk_len);
 	hdrlen = sizeof(*reply) + mic_len + 2;
 	rbuf = wpa_sm_alloc_eapol(sm, IEEE802_1X_TYPE_EAPOL_KEY, NULL,
-				  hdrlen, &rlen, (void *) &reply);
+				  hdrlen + kde_len, &rlen, (void *) &reply);
 	if (rbuf == NULL)
 		return -1;
 
@@ -1651,7 +1685,27 @@  static int wpa_supplicant_send_2_of_2(struct wpa_sm *sm,
 		  WPA_REPLAY_COUNTER_LEN);
 
 	key_mic = (u8 *) (reply + 1);
-	WPA_PUT_BE16(key_mic + mic_len, 0);
+	WPA_PUT_BE16(key_mic + mic_len, kde_len); /* Key Data Length */
+
+#ifdef CONFIG_OCV
+	if (wpa_sm_ocv_enabled(sm)) {
+		struct wpa_channel_info ci;
+		u8 *pos;
+
+		if (wpa_sm_channel_info(sm, &ci) != 0) {
+			wpa_printf(MSG_WARNING, "Failed to get channel info "
+				   "for OCI element in EAPOL-Key 2/2");
+			os_free(rbuf);
+			return -1;
+		}
+
+		pos = key_mic + mic_len + 2; /* Key Data */
+		if (ocv_insert_oci_kde(&ci, &pos) < 0) {
+			os_free(rbuf);
+			return -1;
+		}
+	}
+#endif /* CONFIG_OCV */
 
 	wpa_dbg(sm->ctx->msg_ctx, MSG_DEBUG, "WPA: Sending EAPOL-Key 2/2");
 	return wpa_eapol_key_send(sm, &sm->ptk, ver, sm->bssid, ETH_P_EAPOL,
@@ -2923,6 +2977,21 @@  int wpa_sm_pmf_enabled(struct wpa_sm *sm)
 }
 
 
+int wpa_sm_ocv_enabled(struct wpa_sm *sm)
+{
+	struct wpa_ie_data rsn;
+
+	if (!sm->ocv || !sm->ap_rsn_ie)
+		return 0;
+
+	if (wpa_parse_wpa_ie_rsn(sm->ap_rsn_ie, sm->ap_rsn_ie_len, &rsn) >= 0 &&
+	    rsn.capabilities & WPA_CAPABILITY_OCVC)
+		return 1;
+
+	return 0;
+}
+
+
 /**
  * wpa_sm_set_assoc_wpa_ie_default - Generate own WPA/RSN IE from configuration
  * @sm: Pointer to WPA state machine data from wpa_sm_init()
diff --git a/src/rsn_supp/wpa.h b/src/rsn_supp/wpa.h
index b832267a5..9eee383de 100644
--- a/src/rsn_supp/wpa.h
+++ b/src/rsn_supp/wpa.h
@@ -144,6 +144,7 @@  int wpa_sm_set_param(struct wpa_sm *sm, enum wpa_sm_conf_params param,
 int wpa_sm_get_status(struct wpa_sm *sm, char *buf, size_t buflen,
 		      int verbose);
 int wpa_sm_pmf_enabled(struct wpa_sm *sm);
+int wpa_sm_ocv_enabled(struct wpa_sm *sm);
 
 void wpa_sm_key_request(struct wpa_sm *sm, int error, int pairwise);
 
@@ -282,6 +283,11 @@  static inline int wpa_sm_pmf_enabled(struct wpa_sm *sm)
 	return 0;
 }
 
+static inline int wpa_sm_ocv_enabled(struct wpa_sm *sm)
+{
+	return 0;
+}
+
 static inline void wpa_sm_key_request(struct wpa_sm *sm, int error,
 				      int pairwise)
 {