/*
|
* Copyright (c) 2015 South Silicon Valley Microelectronics Inc.
|
* Copyright (c) 2015 iComm Corporation
|
*
|
* This program is free software: you can redistribute it and/or modify
|
* it under the terms of the GNU General Public License as published by
|
* the Free Software Foundation, either version 3 of the License, or
|
* (at your option) any later version.
|
* This program is distributed in the hope that it will be useful, but
|
* WITHOUT ANY WARRANTY; without even the implied warranty of
|
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
|
* See the GNU General Public License for more details.
|
* You should have received a copy of the GNU General Public License
|
* along with this program. If not, see <http://www.gnu.org/licenses/>.
|
*/
|
|
#include <linux/kernel.h>
|
#include <linux/string.h>
|
#include <net/cfg80211.h>
|
#include <net/mac80211.h>
|
#include <linux/etherdevice.h>
|
#include "wapi_sms4.h"
|
#include "sec_wpi.h"
|
#include "sec.h"
|
#define IWAPIELEMENT 68
|
#define WID_WAPI_KEY 0x3033
|
u8 g_wapi_oui[3] = {0x00,0x14,0x72};
|
const u16 frame_cntl_mask = 0x8FC7;
|
const u16 seq_cntl_mask = 0x0F00;
|
struct lib80211_wpi_data {
|
TRUTH_VALUE_T wapi_enable;
|
TRUTH_VALUE_T wapi_key_ok;
|
u8 wapi_version[2];
|
u8 ap_address[ETH_ALEN];
|
u8 key_index;
|
u8 pn_key[WAPI_PN_LEN];
|
u8 pmsk_key[3][WAPI_PN_LEN];
|
u8 mic_key[3][WAPI_PN_LEN];
|
};
|
TRUTH_VALUE_T mget_wapi_key_ok(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return data->wapi_key_ok;
|
}
|
void mset_wapi_key_ok(TRUTH_VALUE_T val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
data->wapi_key_ok = val;
|
}
|
TRUTH_VALUE_T mget_wapi_enable(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return data->wapi_enable;
|
}
|
void mset_wapi_enable(TRUTH_VALUE_T val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
data->wapi_enable = val;
|
}
|
u8* mget_wapi_version(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return data->wapi_version;
|
}
|
void mset_wapi_version(u8* val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
memcpy(data->wapi_version,val,2);
|
}
|
u8* mget_wapi_address(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return data->ap_address;
|
}
|
void mset_wapi_address(u8* val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
memcpy(data->ap_address,val, ETH_ALEN);
|
}
|
u8 mget_key_index(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return data->key_index;
|
}
|
void mset_key_index(int index, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
if(index <= 3 )
|
{
|
data->key_index = index;
|
}
|
}
|
u8* mget_pn_key(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return data->pn_key;
|
}
|
void mset_pn_key(u8* val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
memcpy(data->pn_key,val,WAPI_PN_LEN);
|
}
|
u8 *inc_pn_key(void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
int i;
|
data->pn_key[15] += 2;
|
if( data->pn_key[15] == 0x00 )
|
{
|
for(i = 14 ; i >= 0 ; i--)
|
{
|
if( (data->pn_key[i] += 1) != 0x00 )
|
{
|
break;
|
}
|
}
|
}
|
return data->pn_key;
|
}
|
u8 *mget_pmsk_key(int index, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return ( index >= 3 ) ? NULL : data->pmsk_key[index];
|
}
|
void mset_pmsk_key(int index,u8* val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
if(index < 3 )
|
{
|
memcpy(data->pmsk_key[index],val, WAPI_MIC_LEN);
|
}
|
}
|
u8* mget_mic_key(int index, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
return ( index >= 3 ) ? NULL : data->mic_key[index];
|
}
|
void mset_mic_key(int index,u8* val, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
if(index < 3 )
|
{
|
memcpy(data->mic_key[index],val,WAPI_MIC_LEN);
|
}
|
}
|
u16 wlan_tx_wapi_encryption(u8 * header,
|
u8 * data,u16 data_len,
|
u8 * mic_pos, void *priv)
|
{
|
int i = 0;
|
u16 offset = 0;
|
BOOL_T qos_in = BFALSE;
|
BOOL_T valid_addr4 = BTRUE;
|
u8 ptk_header[36] = {0};
|
u16 ptk_headr_len = 32;
|
u8 *p_ptk_header = ptk_header;
|
u8 *data_mic = mic_pos;
|
u8 *iv = NULL;
|
u8 keyid = 0;
|
keyid = mget_key_index(priv);
|
#ifdef MULTI_THREAD_ENCRYPT
|
iv = kzalloc(WAPI_PN_LEN, GFP_KERNEL);
|
memcpy(iv, data + WAPI_KEYID_LEN + WAPI_RESERVD_LEN, WAPI_PN_LEN);
|
data_len -= WAPI_IV_LEN;
|
#else
|
iv = inc_pn_key(priv);
|
#endif
|
*data = keyid;
|
*(data + 1) = 0x00;
|
data += 2;
|
for( i = 15 ; i >= 0 ; i-- ) {
|
*data = iv[i];
|
data++;
|
}
|
*p_ptk_header = header[offset] & (frame_cntl_mask >> 8);
|
*(p_ptk_header + 1) = header[offset + 1] & (frame_cntl_mask & 0xFF);
|
if(*p_ptk_header & 0x80) {
|
qos_in = BTRUE;
|
ptk_headr_len += 2;
|
}
|
if((*(p_ptk_header + 1) & 0x03 ) != 0x03) {
|
valid_addr4 = BFALSE;
|
}
|
p_ptk_header += 2;
|
offset += 2;
|
offset += 2;
|
memcpy(p_ptk_header, &header[offset], ADDID_LEN);
|
p_ptk_header += ADDID_LEN;
|
offset += ADDID_LEN;
|
*p_ptk_header = header[offset + ETH_ALEN] & (seq_cntl_mask >> 8);
|
*(p_ptk_header + 1) = header[offset + ETH_ALEN + 1] & (seq_cntl_mask & 0xFF);
|
p_ptk_header += 2;
|
memcpy(p_ptk_header, &header[offset], ETH_ALEN);
|
p_ptk_header += ETH_ALEN;
|
offset += ETH_ALEN;
|
offset += 2;
|
if(valid_addr4) {
|
memcpy(p_ptk_header, &header[offset], ETH_ALEN);
|
p_ptk_header += ETH_ALEN;
|
offset += ETH_ALEN;
|
}
|
else {
|
memset(p_ptk_header,0x00, ETH_ALEN);
|
p_ptk_header += ETH_ALEN;
|
}
|
if(qos_in) {
|
memcpy(p_ptk_header, &header[offset], 2);
|
p_ptk_header += 2;
|
offset += 2;
|
}
|
*p_ptk_header = keyid;
|
p_ptk_header++;
|
*p_ptk_header = 0x00;
|
p_ptk_header++;
|
*p_ptk_header = (data_len >> 8);
|
*(p_ptk_header+1) = data_len & 0xFF;
|
WapiCryptoSms4Mic(iv,
|
mget_mic_key(keyid, priv),
|
ptk_header, ptk_headr_len, data, data_len, data_mic);
|
data_len += WAPI_MIC_LEN;
|
WapiCryptoSms4(iv,
|
mget_pmsk_key(keyid, priv),
|
data, data_len,
|
data);
|
#ifdef MULTI_THREAD_ENCRYPT
|
kfree(iv);
|
#endif
|
return data_len + WAPI_IV_LEN;
|
}
|
BOOL_T is_group(u8* addr)
|
{
|
if((addr[0] & BIT(0)) != 0)
|
return BTRUE;
|
return BFALSE;
|
}
|
u16 wlan_rx_wapi_decryption(u8 * input_ptk,u16 header_len,u16 data_len,
|
u8 * output_buf, void *priv)
|
{
|
u16 offset = 0;
|
BOOL_T qos_in = BFALSE;
|
BOOL_T valid_addr4 = BTRUE;
|
BOOL_T is_group_ptk = BFALSE;
|
u8 ptk_header[36] = {0};
|
u16 ptk_headr_len = 32;
|
u8 * p_ptk_header = ptk_header;
|
u8 data_mic[WAPI_MIC_LEN] = {0};
|
u8 calc_data_mic[WAPI_MIC_LEN] = {0};
|
u8 iv[WAPI_PN_LEN] = {0};
|
u8 keyid = {0};
|
u16 ral_data_len = 0;
|
u16 encryp_data_len = 0;
|
int i = 0;
|
*p_ptk_header = input_ptk[offset] & (frame_cntl_mask >> 8);
|
*(p_ptk_header+1) = input_ptk[offset+1] & (frame_cntl_mask & 0xFF);
|
if(*p_ptk_header & 0x80) {
|
qos_in = BTRUE;
|
ptk_headr_len += 2;
|
}
|
if((*(p_ptk_header+1) & 0x03 ) != 0x03) {
|
valid_addr4 = BFALSE;
|
}
|
p_ptk_header += 2;
|
offset += 2;
|
offset += 2;
|
memcpy(p_ptk_header, &input_ptk[offset], ADDID_LEN);
|
is_group_ptk = is_group(p_ptk_header);
|
p_ptk_header += ADDID_LEN;
|
offset += ADDID_LEN;
|
*p_ptk_header = input_ptk[offset+6] & (seq_cntl_mask >> 8);
|
*(p_ptk_header+1) = input_ptk[offset+6+1] & (seq_cntl_mask & 0xFF);
|
p_ptk_header += 2;
|
memcpy(p_ptk_header, &input_ptk[offset], ETH_ALEN);
|
p_ptk_header += ETH_ALEN;
|
offset += ETH_ALEN;
|
offset += 2;
|
if(valid_addr4) {
|
memcpy(p_ptk_header, &input_ptk[offset], ETH_ALEN);
|
p_ptk_header += ETH_ALEN;
|
offset += ETH_ALEN;
|
}
|
else {
|
memset(p_ptk_header, 0x00, ETH_ALEN);
|
p_ptk_header += ETH_ALEN;
|
}
|
if(qos_in) {
|
memcpy(p_ptk_header,&input_ptk[offset], 2);
|
p_ptk_header += 2;
|
offset += 2;
|
}
|
*p_ptk_header = input_ptk[offset];
|
keyid = input_ptk[offset];
|
p_ptk_header++;
|
offset++;
|
*p_ptk_header = input_ptk[offset];
|
p_ptk_header++;
|
offset++;
|
encryp_data_len = data_len - WAPI_IV_LEN;
|
ral_data_len = data_len - WAPI_IV_LEN - WAPI_MIC_LEN;
|
*p_ptk_header = (ral_data_len >> 8);
|
*(p_ptk_header+1) = ral_data_len & 0xFF;
|
for( i = 15 ; i >= 0 ; i-- ) {
|
iv[i] = input_ptk[offset];
|
offset++;
|
}
|
if(is_group_ptk) {
|
printk("%s, is_group_ptk\n", __func__);
|
}
|
else {
|
if( (iv[15] & 0x01) != 0x01 ) {
|
printk(KERN_ALERT "decry pairwise error,iv[15]=%x\n", iv[15]);
|
return 0;
|
}
|
}
|
WapiCryptoSms4(iv,
|
mget_pmsk_key(keyid, priv),
|
(input_ptk + header_len + WAPI_IV_LEN), encryp_data_len,
|
output_buf);
|
memcpy(data_mic, output_buf + ral_data_len, WAPI_MIC_LEN);
|
WapiCryptoSms4Mic(iv,
|
mget_mic_key(keyid, priv),
|
ptk_header, ptk_headr_len,
|
(output_buf), ral_data_len,
|
calc_data_mic);
|
if( memcmp(calc_data_mic, data_mic, WAPI_MIC_LEN) != 0 ) {
|
printk(KERN_ALERT "calc_data_mic != data_mic\n");
|
return 0;
|
}
|
else {
|
return ral_data_len;
|
}
|
}
|
int lib80211_wpi_encrypt(struct sk_buff *mpdu, int hdr_len, void *priv)
|
{
|
struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)mpdu->data;
|
u8 *pos, *mic_pos;
|
int hdrlen = 0, len = 0, out_len = 0;
|
u8 *pdata = NULL;
|
#ifdef MULTI_THREAD_ENCRYPT
|
u32 wapi_iv_icv_offset = WAPI_IV_ICV_OFFSET - WAPI_IV_LEN;
|
#else
|
u32 wapi_iv_icv_offset = WAPI_IV_ICV_OFFSET;
|
#endif
|
hdrlen = ieee80211_hdrlen(hdr->frame_control);
|
pdata = (mpdu->data) + hdrlen;
|
if (mpdu->protocol != cpu_to_be16(0x88b4)) {
|
if (WARN_ON(skb_headroom(mpdu) < wapi_iv_icv_offset)) {
|
printk("[I] skb_headroom(skb) < %d\n", wapi_iv_icv_offset);
|
return 0;
|
}
|
len = mpdu->len - hdrlen;
|
pos = skb_push(mpdu, wapi_iv_icv_offset);
|
#ifdef MULTI_THREAD_ENCRYPT
|
memmove(pos, pos + wapi_iv_icv_offset, mpdu->len - WAPI_MIC_LEN);
|
#else
|
memmove(pos, pos + wapi_iv_icv_offset, hdrlen);
|
memmove(pos + hdrlen + WAPI_IV_LEN, pos + wapi_iv_icv_offset + hdrlen, len);
|
#endif
|
hdr = (struct ieee80211_hdr *)pos;
|
pos += hdrlen;
|
mic_pos = mpdu->data + mpdu->len - WAPI_MIC_LEN;
|
out_len = wlan_tx_wapi_encryption((u8 *)hdr, pos, len, mic_pos, priv);
|
}
|
else {
|
if (ieee80211_has_protected(hdr->frame_control))
|
hdr->frame_control &= ~(cpu_to_le16(IEEE80211_FCTL_PROTECTED));
|
printk("[I] send WAPI WAI data pkt\n");
|
}
|
return 1;
|
}
|
int lib80211_wpi_decrypt(struct sk_buff *rx_skb, int hdr_len, void *priv)
|
{
|
struct ieee80211_hdr *hdr = (struct ieee80211_hdr *)(rx_skb->data);
|
int hdrlen, len, dcry_len;
|
char *pdata = NULL;
|
int ret = 0;
|
hdrlen = ieee80211_hdrlen(hdr->frame_control);
|
pdata = ((char*)(rx_skb->data)) + hdrlen;
|
len = rx_skb->len - hdrlen;
|
dcry_len = wlan_rx_wapi_decryption((u8 *)rx_skb->data, hdrlen, len, pdata, priv);
|
if (dcry_len) {
|
skb_trim(rx_skb, hdrlen + dcry_len);
|
hdr->frame_control &= ~(cpu_to_le16(IEEE80211_FCTL_PROTECTED));
|
ret = dcry_len;
|
}
|
return ret;
|
}
|
void *lib80211_wpi_init(int key_idx)
|
{
|
struct lib80211_wpi_data *priv;
|
priv = kzalloc(sizeof(*priv), GFP_ATOMIC);
|
if (priv == NULL) {
|
printk("allocate lib80211_wpi_data failed\n");
|
return NULL;
|
}
|
priv->key_index = key_idx;
|
return priv;
|
}
|
void lib80211_wpi_deinit(void *priv)
|
{
|
if (priv) {
|
printk("%s\n", __func__);
|
kfree(priv);
|
priv = NULL;
|
}
|
else
|
printk("%s, passing NULL lib80211_wpi_data?\n", __func__);
|
}
|
#ifdef MULTI_THREAD_ENCRYPT
|
int lib80211_wpi_encrypt_prepare(struct sk_buff *mpdu, int hdr_len, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
u8 *pos = NULL;
|
unsigned char *iv = NULL;
|
if (mpdu->protocol != cpu_to_be16(0x88b4) &&
|
(skb_headroom(mpdu) >= WAPI_IV_LEN)) {
|
pos = skb_push(mpdu, WAPI_IV_LEN);
|
memmove(pos, pos + WAPI_IV_LEN, hdr_len);
|
pos += hdr_len;
|
*pos = data->key_index;
|
pos++;
|
*pos = 0x00;
|
pos++;
|
iv = inc_pn_key(priv);
|
memcpy(pos, iv, WAPI_PN_LEN);
|
return 0;
|
}
|
printk("%s, pass through\n", __func__);
|
return 0;
|
}
|
#endif
|
int lib80211_wpi_set_key(void *key, int len, u8 *seq, void *priv)
|
{
|
struct lib80211_wpi_data *data = priv;
|
int keyidx = data->key_index;
|
u8 WapiASUEPNInitialValueSrc[16] = {
|
0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36,
|
0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36, 0x5C, 0x36
|
};
|
printk("%s\n", __func__);
|
mset_key_index(keyidx, priv);
|
mset_pn_key(WapiASUEPNInitialValueSrc, priv);
|
mset_pmsk_key(keyidx, key, priv);
|
mset_mic_key(keyidx, key + WAPI_PN_LEN, priv);
|
if (seq) {
|
memcpy(data->ap_address, (u8 *)seq, ETH_ALEN);
|
printk("%s: set ap_address %pM\n", __func__, data->ap_address);
|
}
|
mset_wapi_key_ok(TV_TRUE, priv);
|
return 0;
|
}
|
static struct ssv_crypto_ops ssv_crypto_wpi = {
|
.name = "WPI",
|
.init = lib80211_wpi_init,
|
.deinit = lib80211_wpi_deinit,
|
.encrypt_mpdu = lib80211_wpi_encrypt,
|
.decrypt_mpdu = lib80211_wpi_decrypt,
|
.encrypt_msdu = NULL,
|
.decrypt_msdu = NULL,
|
.set_tx_pn = NULL,
|
.set_key = lib80211_wpi_set_key,
|
.get_key = NULL,
|
.print_stats = NULL,
|
.extra_mpdu_prefix_len = WAPI_IV_LEN,
|
.extra_mpdu_postfix_len = WAPI_MIC_LEN,
|
#ifdef MULTI_THREAD_ENCRYPT
|
.encrypt_prepare = lib80211_wpi_encrypt_prepare,
|
.decrypt_prepare = NULL,
|
#endif
|
};
|
struct ssv_crypto_ops *get_crypto_wpi_ops(void)
|
{
|
return &ssv_crypto_wpi;
|
}
|
