qcacmn: Fix ccmp sw encrypt and decrypt

Fixed ccmp sw encrypt and decrypt

Change-Id: Ic0bff6cb764747f31fbb2354e170978144f178d6
Crs-Fixed: 2204858

umac/cmn_services/crypto/src/wlan_crypto_aes_i.h

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2018 The Linux Foundation. All rights reserved.
  */
 /*
  * AES (Rijndael) cipher
@@ -22,6 +22,9 @@
 #include "wlan_crypto_global_def.h"
 
 
+#define CCMP_IV_SIZE (WLAN_CRYPTO_IV_LEN + WLAN_CRYPTO_KEYID_LEN \
+				+ WLAN_CRYPTO_EXT_IV_LEN)
+
 #define AES_BLOCK_SIZE 16
 #define wpa_printf(args...) do { } while (0)
 #define wpa_hexdump(l, t, b, le) do { } while (0)

umac/cmn_services/crypto/src/wlan_crypto_ccmp.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2018 The Linux Foundation. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for
  * any purpose with or without fee is hereby granted, provided that the
@@ -73,14 +73,14 @@ static QDF_STATUS ccmp_encap(struct wlan_crypto_key *key,
 	/* XXX wrap at 48 bits */
 	key->keytsc++;
 
-	ivp[0] = key->keytsc >> 0;         /* PN0 */
-	ivp[1] = key->keytsc >> 8;         /* PN1 */
-	ivp[2] = 0;                         /* Reserved */
-	ivp[3] = key->keyix | WLAN_CRYPTO_EXT_IV_BIT;       /* KeyID | ExtID */
-	ivp[4] = key->keytsc >> 16;                /* PN2 */
-	ivp[5] = key->keytsc >> 24;                /* PN3 */
-	ivp[6] = key->keytsc >> 32;                /* PN4 */
-	ivp[7] = key->keytsc >> 40;                /* PN5 */
+	ivp[0] = key->keytsc >> 0;                         /* PN0 */
+	ivp[1] = key->keytsc >> 8;                         /* PN1 */
+	ivp[2] = 0;                                        /* Reserved */
+	ivp[3] = (key->keyix << 6)| WLAN_CRYPTO_EXT_IV_BIT;/* KeyID | ExtID */
+	ivp[4] = key->keytsc >> 16;                        /* PN2 */
+	ivp[5] = key->keytsc >> 24;                        /* PN3 */
+	ivp[6] = key->keytsc >> 32;                        /* PN4 */
+	ivp[7] = key->keytsc >> 40;                        /* PN5 */
 
 	/*
 	 * Finally, do software encrypt if neeed.
@@ -89,7 +89,7 @@ static QDF_STATUS ccmp_encap(struct wlan_crypto_key *key,
 		if (!wlan_crypto_ccmp_encrypt(key->keyval,
 						qdf_nbuf_data(wbuf),
 						qdf_nbuf_len(wbuf), hdrlen)) {
-			return 0;
+			return QDF_STATUS_CRYPTO_ENCRYPT_FAILED;
 		}
 	}
 
@@ -204,9 +204,15 @@ static QDF_STATUS ccmp_decap(struct wlan_crypto_key *key,
 	/*
 	 * Copy up 802.11 header and strip crypto bits.
 	 */
-	qdf_mem_move(origHdr + cipher_table->header, origHdr, hdrlen);
-	qdf_nbuf_pull_head(wbuf, cipher_table->header);
-	qdf_nbuf_trim_tail(wbuf, cipher_table->trailer);
+	if (!(key->flags & WLAN_CRYPTO_KEY_SWDECRYPT)) {
+		qdf_mem_move(origHdr + cipher_table->header, origHdr, hdrlen);
+		qdf_nbuf_pull_head(wbuf, cipher_table->header);
+		qdf_nbuf_trim_tail(wbuf, cipher_table->trailer
+						+ cipher_table->miclen);
+	} else {
+		qdf_nbuf_trim_tail(wbuf, cipher_table->header
+					+ cipher_table->miclen);
+	}
 
 	if (update_keyrsc) {
 		/*

umac/cmn_services/crypto/src/wlan_crypto_ccmp_sw.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2018 The Linux Foundation. All rights reserved.
  */
 /*
  * CTR with CBC-MAC Protocol (CCMP)
@@ -85,7 +85,7 @@ uint8_t *wlan_crypto_ccmp_decrypt(const uint8_t *tk,
 	size_t mlen;
 	uint8_t *plain;
 
-	if (data_len < 8 + 8)
+	if (data_len < CCMP_IV_SIZE + WLAN_CRYPTO_MIC_LEN)
 		return NULL;
 
 	plain = qdf_mem_malloc(data_len + AES_BLOCK_SIZE);
@@ -94,15 +94,19 @@ uint8_t *wlan_crypto_ccmp_decrypt(const uint8_t *tk,
 		return NULL;
 	}
 
-	mlen = data_len - 8 - 8;
+	mlen = data_len - CCMP_IV_SIZE - WLAN_CRYPTO_MIC_LEN;
 
 	qdf_mem_set(aad, sizeof(aad), 0);
 	ccmp_aad_nonce(hdr, data, aad, &aad_len, nonce);
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", aad, aad_len);
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);
 
-	if (wlan_crypto_aes_ccm_ad(tk, 16, nonce, 8, data + 8, mlen,
-				aad, aad_len, data + 8 + mlen, plain) < 0) {
+	if (wlan_crypto_aes_ccm_ad(tk, 16, nonce,
+					WLAN_CRYPTO_MIC_LEN,
+					data + CCMP_IV_SIZE, mlen,
+					aad, aad_len,
+					data + CCMP_IV_SIZE + mlen,
+					plain) < 0) {
 		/*uint16_t seq_ctrl = qdf_le16_to_cpu(hdr->seq_ctrl);
 		wpa_printf(MSG_INFO, "Invalid CCMP MIC in frame: A1=" MACSTR
 			" A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
@@ -141,15 +145,16 @@ uint8_t *wlan_crypto_ccmp_encrypt(const uint8_t *tk, uint8_t *frame,
 
 	if (len < hdrlen || hdrlen < 24)
 		return NULL;
-	plen = len - hdrlen;
+	plen = len - hdrlen - CCMP_IV_SIZE - WLAN_CRYPTO_MIC_LEN;
 
-	crypt = qdf_mem_malloc(hdrlen + 8 + plen + 8 + AES_BLOCK_SIZE);
+	crypt = qdf_mem_malloc(hdrlen + CCMP_IV_SIZE + plen
+				+ WLAN_CRYPTO_MIC_LEN + AES_BLOCK_SIZE);
 	if (crypt == NULL) {
 		qdf_print("%s[%d] mem alloc failed\n", __func__, __LINE__);
 		return NULL;
 	}
 
-	qdf_mem_copy(crypt, frame, hdrlen);
+	qdf_mem_copy(crypt, frame, hdrlen + CCMP_IV_SIZE);
 
 	hdr = (struct ieee80211_hdr *) crypt;
 	hdr->frame_control |= qdf_cpu_to_le16(WLAN_FC_ISWEP);
@@ -160,14 +165,16 @@ uint8_t *wlan_crypto_ccmp_encrypt(const uint8_t *tk, uint8_t *frame,
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP AAD", aad, aad_len);
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP nonce", nonce, 13);
 
-	if (wlan_crypto_aes_ccm_ae(tk, 16, nonce, 8, frame + hdrlen,
+	if (wlan_crypto_aes_ccm_ae(tk, 16, nonce, WLAN_CRYPTO_MIC_LEN,
+					frame + hdrlen + CCMP_IV_SIZE,
 			plen, aad, aad_len, pos, pos + plen) < 0) {
 		qdf_mem_free(crypt);
 		return NULL;
 	}
 
 	qdf_mem_copy(frame, crypt, len);
-	wpa_hexdump(MSG_EXCESSIVE, "CCMP encrypted", crypt + hdrlen + 8, plen);
+	wpa_hexdump(MSG_EXCESSIVE, "CCMP encrypted",
+				crypt + hdrlen + CCMP_IV_SIZE, plen);
 	qdf_mem_free(crypt);
 
 	return frame;
@@ -183,7 +190,7 @@ uint8_t *wlan_crypto_ccmp_256_decrypt(const uint8_t *tk,
 	size_t mlen;
 	uint8_t *plain;
 
-	if (data_len < 8 + 16)
+	if (data_len < CCMP_IV_SIZE + WLAN_CRYPTO_MIC256_LEN)
 		return NULL;
 
 	plain = qdf_mem_malloc(data_len + AES_BLOCK_SIZE);
@@ -192,15 +199,18 @@ uint8_t *wlan_crypto_ccmp_256_decrypt(const uint8_t *tk,
 		return NULL;
 	}
 
-	mlen = data_len - 8 - 16;
+	mlen = data_len - CCMP_IV_SIZE - WLAN_CRYPTO_MIC256_LEN;
 
 	qdf_mem_set(aad, sizeof(aad), 0);
 	ccmp_aad_nonce(hdr, data, aad, &aad_len, nonce);
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 AAD", aad, aad_len);
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 nonce", nonce, 13);
 
-	if (wlan_crypto_aes_ccm_ad(tk, 32, nonce, 16, data + 8, mlen,
-			aad, aad_len, data + 8 + mlen, plain) < 0) {
+	if (wlan_crypto_aes_ccm_ad(tk, 32, nonce, WLAN_CRYPTO_MIC256_LEN,
+					data + CCMP_IV_SIZE, mlen,
+					aad, aad_len,
+					data + CCMP_IV_SIZE + mlen,
+					plain) < 0) {
 		/*uint16_t seq_ctrl = qdf_le16_to_cpu(hdr->seq_ctrl);
 		wpa_printf(MSG_INFO, "Invalid CCMP-256 MIC in frame: A1=" MACSTR
 			   " A2=" MACSTR " A3=" MACSTR " seq=%u frag=%u",
@@ -231,7 +241,8 @@ uint8_t *wlan_crypto_ccmp_256_encrypt(const uint8_t *tk, uint8_t *frame,
 		return NULL;
 	plen = len - hdrlen;
 
-	crypt = qdf_mem_malloc(hdrlen + 8 + plen + 16 + AES_BLOCK_SIZE);
+	crypt = qdf_mem_malloc(hdrlen + CCMP_IV_SIZE + plen
+				+ WLAN_CRYPTO_MIC256_LEN + AES_BLOCK_SIZE);
 	if (crypt == NULL) {
 		qdf_print("%s[%d] mem alloc failed\n", __func__, __LINE__);
 		return NULL;
@@ -255,7 +266,8 @@ uint8_t *wlan_crypto_ccmp_256_encrypt(const uint8_t *tk, uint8_t *frame,
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 AAD", aad, aad_len);
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 nonce", nonce, 13);
 
-	if (wlan_crypto_aes_ccm_ae(tk, 32, nonce, 16, frame + hdrlen, plen,
+	if (wlan_crypto_aes_ccm_ae(tk, 32, nonce, WLAN_CRYPTO_MIC256_LEN,
+				frame + hdrlen, plen,
 				aad, aad_len, pos, pos + plen) < 0) {
 		qdf_mem_free(crypt);
 		return NULL;
@@ -264,7 +276,8 @@ uint8_t *wlan_crypto_ccmp_256_encrypt(const uint8_t *tk, uint8_t *frame,
 	wpa_hexdump(MSG_EXCESSIVE, "CCMP-256 encrypted", crypt + hdrlen + 8,
 		    plen);
 
-	*encrypted_len = hdrlen + 8 + plen + 16;
+	*encrypted_len = hdrlen + CCMP_IV_SIZE
+				+ plen + WLAN_CRYPTO_MIC256_LEN;
 
 	return crypt;
 }

umac/cmn_services/crypto/src/wlan_crypto_def_i.h

@@ -493,6 +493,26 @@ static inline int ieee80211_hdrsize(const void *data)
 	return size;
 }
 
+/**
+ * wlan_crypto_get_keyid - get keyid from frame
+ * @data: frame
+ *
+ * This function parse frame and returns keyid
+ *
+ * Return: keyid
+ */
+static inline uint16_t wlan_crypto_get_keyid(uint8_t *data)
+{
+	struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)data;
+
+	if (hdr->frame_control & WLAN_FC_ISWEP) {
+		uint8_t *iv;
+		iv = data + ieee80211_hdrsize(data);
+		return ((iv[3] >> 6) & 0x3);
+	} else
+		return WLAN_CRYPTO_KEYIX_NONE;
+}
+
 /**
  * wlan_get_tid - get tid of the frame
  * @data: frame

umac/cmn_services/crypto/src/wlan_crypto_global_api.c

@@ -266,7 +266,6 @@ int32_t wlan_crypto_get_param(struct wlan_objmgr_vdev *vdev,
 
 	return value;
 }
-
 /**
  * wlan_crypto_get_peer_param - called to get value for param from peer
  * @peer:  peer
@@ -1093,6 +1092,7 @@ QDF_STATUS wlan_crypto_decap(struct wlan_objmgr_vdev *vdev,
 	struct wlan_objmgr_psoc *psoc;
 	struct wlan_objmgr_peer *peer;
 	uint8_t bssid_mac[WLAN_ALEN];
+	uint8_t keyid;
 
 	wlan_vdev_obj_lock(vdev);
 	qdf_mem_copy(bssid_mac, wlan_vdev_mlme_get_macaddr(vdev), WLAN_ALEN);
@@ -1104,6 +1104,11 @@ QDF_STATUS wlan_crypto_decap(struct wlan_objmgr_vdev *vdev,
 	}
 	wlan_vdev_obj_unlock(vdev);
 
+	keyid = wlan_crypto_get_keyid((uint8_t *)qdf_nbuf_data(wbuf));
+
+	if (keyid >= WLAN_CRYPTO_MAXKEYIDX)
+		return QDF_STATUS_E_INVAL;
+
 	/* FILS Decap required only for (Re-)Assoc request */
 	peer = wlan_objmgr_get_peer(psoc, mac_addr, WLAN_CRYPTO_ID);
 
@@ -1125,7 +1130,7 @@ QDF_STATUS wlan_crypto_decap(struct wlan_objmgr_vdev *vdev,
 			return QDF_STATUS_E_INVAL;
 		}
 
-		key = crypto_priv->key[crypto_priv->def_tx_keyid];
+		key = crypto_priv->key[keyid];
 		if (!key)
 			return QDF_STATUS_E_INVAL;
 
@@ -1150,7 +1155,7 @@ QDF_STATUS wlan_crypto_decap(struct wlan_objmgr_vdev *vdev,
 			return QDF_STATUS_E_INVAL;
 		}
 
-		key = crypto_priv->key[crypto_priv->def_tx_keyid];
+		key = crypto_priv->key[keyid];
 		if (!key)
 			return QDF_STATUS_E_INVAL;
 	}

umac/cmn_services/crypto/src/wlan_crypto_tkip.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2018 The Linux Foundation. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for
  * any purpose with or without fee is hereby granted, provided that the
@@ -69,7 +69,7 @@ static QDF_STATUS tkip_encap(struct wlan_crypto_key *key,
 	ivp[0] = key->keytsc >> 8;            /* TSC1 */
 	ivp[1] = (ivp[0] | 0x20) & 0x7f;      /* WEP seed */
 	ivp[2] = key->keytsc >> 0;            /* TSC0*/
-	ivp[3] = key->keyix | WLAN_CRYPTO_EXT_IV_BIT; /* KeyID | ExtID */
+	ivp[3] = (key->keyix << 6) | WLAN_CRYPTO_EXT_IV_BIT; /* KeyID | ExtID */
 	ivp[4] = key->keytsc >> 16;           /* PN2 */
 	ivp[5] = key->keytsc >> 24;           /* PN3 */
 	ivp[6] = key->keytsc >> 32;           /* PN4 */

umac/cmn_services/crypto/src/wlan_crypto_wep.c

@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017 The Linux Foundation. All rights reserved.
+ * Copyright (c) 2017-2018 The Linux Foundation. All rights reserved.
  *
  * Permission to use, copy, modify, and/or distribute this software for
  * any purpose with or without fee is hereby granted, provided that the
@@ -80,7 +80,7 @@ static QDF_STATUS wep_encap(struct wlan_crypto_key *key,
 	ivp[1] = key->keyrsc[0] >> 8;
 	ivp[2] = key->keyrsc[0] >> 16;
 #endif
-	ivp[3] = key->keyix;
+	ivp[3] = key->keyix << 6;
 
 	/*
 	 * Finally, do software encrypt if neeed.